From 783a0ff972d1fd061f637800c730b2287f7902b4 Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Sat, 20 Jan 2024 22:15:00 -0500 Subject: [PATCH] radial nodes: should all sub to raw topics --- 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_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 ++-- 7 files changed, 8 insertions(+), 12 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_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(