Skip to content
This repository has been archived by the owner on Jan 7, 2023. It is now read-only.

Use default sensor qos for sensor topics #107

Open
wants to merge 1 commit into
base: eloquent
Choose a base branch
from
Open

Use default sensor qos for sensor topics #107

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 6 additions & 5 deletions realsense_ros/src/rs_base.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -94,17 +94,18 @@ void RealSenseBase::setupStream(const stream_index_pair & stream)
enable = node_.declare_parameter(os.str(), DEFAULT_ENABLE_STREAM);
}

rclcpp::SensorDataQoS qos;
if (stream == ACCEL || stream == GYRO) {
imu_pub_.insert(std::pair<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr>
(stream, node_.create_publisher<sensor_msgs::msg::Imu>(SAMPLE_TOPIC.at(stream), rclcpp::QoS(1))));
(stream, node_.create_publisher<sensor_msgs::msg::Imu>(SAMPLE_TOPIC.at(stream), qos)));
imu_info_pub_.insert(std::pair<stream_index_pair, rclcpp::Publisher<realsense_msgs::msg::IMUInfo>::SharedPtr>
(stream, node_.create_publisher<realsense_msgs::msg::IMUInfo>(INFO_TOPIC.at(stream), rclcpp::QoS(1))));
(stream, node_.create_publisher<realsense_msgs::msg::IMUInfo>(INFO_TOPIC.at(stream), qos)));
if (enable == true) {
enable_[stream] = true;
cfg_.enable_stream(stream.first, stream.second);
}
} else if (stream == POSE) {
odom_pub_ = node_.create_publisher<nav_msgs::msg::Odometry>(SAMPLE_TOPIC.at(stream), rclcpp::QoS(1));
odom_pub_ = node_.create_publisher<nav_msgs::msg::Odometry>(SAMPLE_TOPIC.at(stream), qos);
if (enable == true) {
enable_[stream] = true;
cfg_.enable_stream(stream.first, stream.second);
Expand Down Expand Up @@ -149,9 +150,9 @@ void RealSenseBase::setupStream(const stream_index_pair & stream)

stream_info_.insert(std::pair<stream_index_pair, VideoStreamInfo>(stream, info));
image_pub_.insert(std::pair<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr>
(stream, node_.create_publisher<sensor_msgs::msg::Image>(SAMPLE_TOPIC.at(stream), rclcpp::QoS(1))));
(stream, node_.create_publisher<sensor_msgs::msg::Image>(SAMPLE_TOPIC.at(stream), qos)));
camera_info_pub_.insert(std::pair<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr>
(stream, node_.create_publisher<sensor_msgs::msg::CameraInfo>(INFO_TOPIC.at(stream), rclcpp::QoS(1))));
(stream, node_.create_publisher<sensor_msgs::msg::CameraInfo>(INFO_TOPIC.at(stream), qos)));
if (enable == true) {
enable_[stream] = true;
cfg_.enable_stream(stream.first, stream.second, info.width, info.height, STREAM_FORMAT.at(stream.first), info.fps);
Expand Down
7 changes: 4 additions & 3 deletions realsense_ros/src/rs_d435.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,8 +35,9 @@ RealSenseD435::RealSenseD435(rs2::context ctx, rs2::device dev, rclcpp::Node & n
RCLCPP_WARN(node_.get_logger(), "Make sure color and depth frame are enabled before aligning depth to color.");
}
}
aligned_depth_image_pub_ = node_.create_publisher<sensor_msgs::msg::Image>(ALIGNED_DEPTH_IMAGE_TOPIC, rclcpp::QoS(1));
aligned_depth_info_pub_ = node_.create_publisher<sensor_msgs::msg::CameraInfo>(ALIGNED_DEPTH_INFO_TOPIC, rclcpp::QoS(1));
rclcpp::SensorDataQoS qos;
aligned_depth_image_pub_ = node_.create_publisher<sensor_msgs::msg::Image>(ALIGNED_DEPTH_IMAGE_TOPIC, qos);
aligned_depth_info_pub_ = node_.create_publisher<sensor_msgs::msg::CameraInfo>(ALIGNED_DEPTH_INFO_TOPIC, qos);

if (node_.has_parameter("enable_pointcloud")) {
node_.get_parameter("enable_pointcloud", enable_pointcloud_);
Expand All @@ -48,7 +49,7 @@ RealSenseD435::RealSenseD435(rs2::context ctx, rs2::device dev, rclcpp::Node & n
RCLCPP_WARN(node_.get_logger(), "Make sure color and depth frame are enabled before publishing pointcloud.");
}
}
pointcloud_pub_ = node_.create_publisher<sensor_msgs::msg::PointCloud2>(POINTCLOUD_TOPIC, rclcpp::QoS(1));
pointcloud_pub_ = node_.create_publisher<sensor_msgs::msg::PointCloud2>(POINTCLOUD_TOPIC, qos);
if (node_.has_parameter("dense_pointcloud")) {
node_.get_parameter("dense_pointcloud", dense_pc_);
} else {
Expand Down
7 changes: 4 additions & 3 deletions realsense_ros/tests/camera_core.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -756,15 +756,16 @@ int main(int argc, char * argv[])
auto node = rclcpp::Node::make_shared("realsense_camera");
rmw_qos_profile_t custom_qos = rmw_qos_profile_default;

rclcpp::SensorDataQoS qos;
RCLCPP_INFO(node->get_logger(), "Create a node for %s Camera", camera_type.c_str());
auto depth_sub = image_transport::create_camera_subscription(node.get(), "/camera/depth/image_rect_raw", imageDepthCallback, "raw", custom_qos);
auto color_sub = image_transport::create_camera_subscription(node.get(),"/camera/color/image_raw", imageColorCallback, "raw", custom_qos);
auto infra1_sub = image_transport::create_camera_subscription(node.get(), "/camera/infra1/image_rect_raw", imageInfrared1Callback, "raw", custom_qos);
auto infra2_sub = image_transport::create_camera_subscription(node.get(), "/camera/infra2/image_rect_raw", imageInfrared2Callback, "raw", custom_qos);
auto alginDepth_sub = image_transport::create_camera_subscription(node.get(), "/camera/aligned_depth_to_color/image_raw", imageAlignDepthCallback, "raw", custom_qos);
auto sub_pointcloud = node->create_subscription<sensor_msgs::msg::PointCloud2>("camera/pointcloud", rclcpp::QoS(1), pcCallback);
auto sub_accel = node->create_subscription<sensor_msgs::msg::Imu>("/camera/accel/sample", rclcpp::QoS(1), accelCallback);
auto sub_gyro = node->create_subscription<sensor_msgs::msg::Imu>("/camera/gyro/sample", rclcpp::QoS(1), gyroCallback);
auto sub_pointcloud = node->create_subscription<sensor_msgs::msg::PointCloud2>("camera/pointcloud", qos, pcCallback);
auto sub_accel = node->create_subscription<sensor_msgs::msg::Imu>("/camera/accel/sample", qos, accelCallback);
auto sub_gyro = node->create_subscription<sensor_msgs::msg::Imu>("/camera/gyro/sample", qos, gyroCallback);
auto fisheye1_sub = image_transport::create_camera_subscription(node.get(), "/camera/fisheye1/image_raw", imageFisheye1Callback, "raw", custom_qos);
auto fisheye2_sub = image_transport::create_camera_subscription(node.get(), "/camera/fisheye2/image_raw", imageFisheye2Callback, "raw", custom_qos);

Expand Down