radial nodes: should all sub to raw topics

ros-perception · Jan 21, 2024 · 783a0ff · 783a0ff
1 parent 3dbd2b4
commit 783a0ff
Show file tree

Hide file tree

Showing 7 changed files with 8 additions and 12 deletions.
diff --git a/depth_image_proc/include/depth_image_proc/conversions.hpp b/depth_image_proc/include/depth_image_proc/conversions.hpp
@@ -60,7 +60,7 @@ void convertDepth(
   float center_y = model.cy();
 
   // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
-  double unit_scaling = DepthTraits<T>::toMeters(T(1) );
+  double unit_scaling = DepthTraits<T>::toMeters(T(1));
   float constant_x = unit_scaling / model.fx();
   float constant_y = unit_scaling / model.fy();
   float bad_point = std::numeric_limits<float>::quiet_NaN();

diff --git a/depth_image_proc/launch/point_cloud_xyz_radial.launch.py b/depth_image_proc/launch/point_cloud_xyz_radial.launch.py
@@ -60,8 +60,7 @@ def generate_launch_description():
                     package='depth_image_proc',
                     plugin='depth_image_proc::PointCloudXyzRadialNode',
                     name='point_cloud_xyz_radial_node',
-                    remappings=[('image_raw', '/camera/depth/image_rect_raw'),
-                                ('camera_info', '/camera/depth/camera_info'),
+                    remappings=[('depth/image_raw', '/camera/depth/image_rect_raw'),
                                 ('image', '/camera/depth/converted_image')]
                 ),
             ],

diff --git a/depth_image_proc/launch/point_cloud_xyzrgb_radial.launch.py b/depth_image_proc/launch/point_cloud_xyzrgb_radial.launch.py
@@ -73,10 +73,8 @@ def generate_launch_description():
                     package='depth_image_proc',
                     plugin='depth_image_proc::PointCloudXyzrgbRadialNode',
                     name='point_cloud_xyzrgb_node',
-                    remappings=[('rgb/camera_info', '/camera/color/camera_info'),
-                                ('rgb/image_rect_color', '/camera/color/image_raw'),
-                                ('depth_registered/image_rect',
-                                 '/camera/aligned_depth_to_color/image_raw'),
+                    remappings=[('rgb/image_raw', '/camera/color/image_raw'),
+                                ('depth/image_raw', '/camera/aligned_depth_to_color/image_raw'),
                                 ('points', '/camera/depth_registered/points')]
                 ),
             ],

diff --git a/depth_image_proc/src/point_cloud_xyz.cpp b/depth_image_proc/src/point_cloud_xyz.cpp
@@ -90,7 +90,6 @@ PointCloudXyzNode::PointCloudXyzNode(const rclcpp::NodeOptions & options)
   pub_point_cloud_ = create_publisher<PointCloud2>("points", rclcpp::SensorDataQoS(), pub_options);
 }
 
-
 void PointCloudXyzNode::depthCb(
   const Image::ConstSharedPtr & depth_msg,
   const CameraInfo::ConstSharedPtr & info_msg)

diff --git a/depth_image_proc/src/point_cloud_xyz_radial.cpp b/depth_image_proc/src/point_cloud_xyz_radial.cpp
@@ -68,7 +68,7 @@ PointCloudXyzRadialNode::PointCloudXyzRadialNode(const rclcpp::NodeOptions & opt
         // For compressed topics to remap appropriately, we need to pass a
         // fully expanded and remapped topic name to image_transport
         auto node_base = this->get_node_base_interface();
-        std::string topic = node_base->resolve_topic_or_service_name("image_rect", false);
+        std::string topic = node_base->resolve_topic_or_service_name("depth/image_raw", false);
         // Get transport and QoS
         image_transport::TransportHints depth_hints(this, "raw", "depth_image_transport");
         auto custom_qos = rmw_qos_profile_system_default;

diff --git a/depth_image_proc/src/point_cloud_xyzi_radial.cpp b/depth_image_proc/src/point_cloud_xyzi_radial.cpp
@@ -88,7 +88,7 @@ PointCloudXyziRadialNode::PointCloudXyziRadialNode(const rclcpp::NodeOptions & o
         // fully expanded and remapped topic name to image_transport
         auto node_base = this->get_node_base_interface();
         std::string depth_topic =
-          node_base->resolve_topic_or_service_name("depth/image_rect", false);
+          node_base->resolve_topic_or_service_name("depth/image_raw", false);
         std::string intensity_topic =
           node_base->resolve_topic_or_service_name("intensity/image_raw", false);
         // Allow also remapping camera_info to something different than default

diff --git a/depth_image_proc/src/point_cloud_xyzrgb_radial.cpp b/depth_image_proc/src/point_cloud_xyzrgb_radial.cpp
@@ -103,9 +103,9 @@ PointCloudXyzrgbRadialNode::PointCloudXyzrgbRadialNode(const rclcpp::NodeOptions
         // fully expanded and remapped topic name to image_transport
         auto node_base = this->get_node_base_interface();
         std::string depth_topic =
-          node_base->resolve_topic_or_service_name("depth_registered/image_rect", false);
+          node_base->resolve_topic_or_service_name("depth/image_raw", false);
         std::string rgb_topic =
-          node_base->resolve_topic_or_service_name("rgb/image_rect_color", false);
+          node_base->resolve_topic_or_service_name("rgb/image_raw", false);
         // Allow also remapping camera_info to something different than default
         std::string rgb_info_topic =
           node_base->resolve_topic_or_service_name(