From 98c20c6f705117dfb7c55a750cad390cd244a6c4 Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Mon, 22 Jan 2024 10:39:56 -0500 Subject: [PATCH] radial nodes: should all sub to raw topics (#906) Per findings in https://github.com/ros-perception/image_pipeline/issues/388#issuecomment-1902487162 - instead of renaming xyz_radial and xyzi_radial to image_rect, I should have made the xyzrgb_radial use image_raw (since these nodes use matrices K & D): * Revert the change in xyzi_radial - topic is depth/image_raw as it has always been * Revert the change in xyz_radial, although it is still changed slightly from the old "image_raw" -> "depth/image_raw" for consistency with the other nodes. * Update xyzrgb_radial: * depth_registered/image_rect -> depth/image_raw * rgb/image_rect_color -> rgb/image_raw * update launch files accordingly (and remove camera_info since it no longer needs to be renamed, happens automagically). Note: these launch files are probably epically bad since realsense doesn't output radial images... but we'll leave them as documentation for these nodes. --- depth_image_proc/include/depth_image_proc/conversions.hpp | 2 +- depth_image_proc/launch/point_cloud_xyz_radial.launch.py | 3 +-- depth_image_proc/launch/point_cloud_xyzi.launch.py | 2 +- depth_image_proc/launch/point_cloud_xyzi_radial.launch.py | 2 +- depth_image_proc/launch/point_cloud_xyzrgb_radial.launch.py | 6 ++---- depth_image_proc/src/point_cloud_xyz.cpp | 1 - depth_image_proc/src/point_cloud_xyz_radial.cpp | 2 +- depth_image_proc/src/point_cloud_xyzi_radial.cpp | 2 +- depth_image_proc/src/point_cloud_xyzrgb_radial.cpp | 4 ++-- 9 files changed, 10 insertions(+), 14 deletions(-) diff --git a/depth_image_proc/include/depth_image_proc/conversions.hpp b/depth_image_proc/include/depth_image_proc/conversions.hpp index eef392c63..11496320f 100644 --- 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::toMeters(T(1) ); + double unit_scaling = DepthTraits::toMeters(T(1)); float constant_x = unit_scaling / model.fx(); float constant_y = unit_scaling / model.fy(); float bad_point = std::numeric_limits::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 index d578a05ce..3fc137a5d 100644 --- 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_xyzi.launch.py b/depth_image_proc/launch/point_cloud_xyzi.launch.py index 84c6dc9b8..417a4db69 100644 --- a/depth_image_proc/launch/point_cloud_xyzi.launch.py +++ b/depth_image_proc/launch/point_cloud_xyzi.launch.py @@ -48,7 +48,7 @@ def generate_launch_description(): package='realsense_ros2_camera', node_executable='realsense_ros2_camera', output='screen'), - # TODO: Realsense camera do not support intensity message + # NOTE: Realsense camera do not support intensity message # use color image instead of intensity only for interface test launch_ros.actions.ComposableNodeContainer( name='container', diff --git a/depth_image_proc/launch/point_cloud_xyzi_radial.launch.py b/depth_image_proc/launch/point_cloud_xyzi_radial.launch.py index 721387748..1989e4885 100644 --- a/depth_image_proc/launch/point_cloud_xyzi_radial.launch.py +++ b/depth_image_proc/launch/point_cloud_xyzi_radial.launch.py @@ -48,7 +48,7 @@ def generate_launch_description(): package='realsense_ros2_camera', node_executable='realsense_ros2_camera', output='screen'), - # TODO: Realsense camera do not support intensity message, + # NOTE: Realsense camera do not support intensity message, # use depth instead of intensity only for interface test launch_ros.actions.ComposableNodeContainer( name='container', diff --git a/depth_image_proc/launch/point_cloud_xyzrgb_radial.launch.py b/depth_image_proc/launch/point_cloud_xyzrgb_radial.launch.py index 263e2397c..08008b2bb 100644 --- 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 index 540532b53..0175d929a 100644 --- 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("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 index b08cc5950..660ea99df 100644 --- 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 index ff5066fa8..c592851e0 100644 --- 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 index a02235a35..5211d9754 100644 --- 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(