depth_image_proc: consistent image_transport (ros-perception#900)

 * all node support image_transport and/or depth_image_transport parameters.
 * point cloud nodes use depth_image_transport parameter for all depth inputs
 * fixes so that remapping works appropriately for image topics, even when using transports other than raw
 * fixes so that remapping works appropriately for image_transport outputs (crop/convert nodes)
 * support remapping camera_info topics

depth_image_proc/src/convert_metric.cpp

@@ -35,6 +35,7 @@
 #include <limits>
 #include <memory>
 #include <mutex>
+#include <string>
 
 #include "depth_image_proc/visibility.h"
 
@@ -64,6 +65,9 @@ class ConvertMetricNode : public rclcpp::Node
 ConvertMetricNode::ConvertMetricNode(const rclcpp::NodeOptions & options)
 : Node("ConvertMetricNode", options)
 {
+  // TransportHints does not actually declare the parameter
+  this->declare_parameter<std::string>("image_transport", "raw");
+
   // Create publisher with connect callback
   rclcpp::PublisherOptions pub_options;
   pub_options.event_callbacks.matched_callback =
@@ -73,15 +77,24 @@ ConvertMetricNode::ConvertMetricNode(const rclcpp::NodeOptions & options)
       if (pub_depth_.getNumSubscribers() == 0) {
         sub_raw_.shutdown();
       } else if (!sub_raw_) {
-        image_transport::TransportHints hints(this, "raw");
+        // 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_raw", false);
+        // Get transport hints
+        image_transport::TransportHints hints(this);
         sub_raw_ = image_transport::create_subscription(
-          this, "image_raw",
+          this, topic,
           std::bind(&ConvertMetricNode::depthCb, this, std::placeholders::_1),
           hints.getTransport());
       }
     };
+  // 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", false);
   pub_depth_ =
-    image_transport::create_publisher(this, "image", rmw_qos_profile_default, pub_options);
+    image_transport::create_publisher(this, topic, rmw_qos_profile_default, pub_options);
 }
 
 void ConvertMetricNode::depthCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_msg)

depth_image_proc/src/crop_foremost.cpp

@@ -32,6 +32,7 @@
 
 #include <functional>
 #include <mutex>
+#include <string>
 
 #include "cv_bridge/cv_bridge.hpp"
 #include "depth_image_proc/visibility.h"
@@ -68,6 +69,9 @@ class CropForemostNode : public rclcpp::Node
 CropForemostNode::CropForemostNode(const rclcpp::NodeOptions & options)
 : Node("CropForemostNode", options)
 {
+  // TransportHints does not actually declare the parameter
+  this->declare_parameter<std::string>("image_transport", "raw");
+
   distance_ = this->declare_parameter("distance", 0.0);
 
   // Create publisher with connect callback
@@ -79,15 +83,24 @@ CropForemostNode::CropForemostNode(const rclcpp::NodeOptions & options)
       if (pub_depth_.getNumSubscribers() == 0) {
         sub_raw_.shutdown();
       } else if (!sub_raw_) {
-        image_transport::TransportHints hints(this, "raw");
+        // 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_raw", false);
+        // Get transport hints
+        image_transport::TransportHints hints(this);
         sub_raw_ = image_transport::create_subscription(
-          this, "image_raw",
+          this, topic,
           std::bind(&CropForemostNode::depthCb, this, std::placeholders::_1),
           hints.getTransport());
       }
     };
+  // 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", false);
   pub_depth_ =
-    image_transport::create_publisher(this, "image", rmw_qos_profile_default, pub_options);
+    image_transport::create_publisher(this, topic, rmw_qos_profile_default, pub_options);
 }
 
 void CropForemostNode::depthCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_msg)

depth_image_proc/src/disparity.cpp

@@ -34,6 +34,7 @@
 #include <limits>
 #include <memory>
 #include <mutex>
+#include <string>
 
 #include "depth_image_proc/visibility.h"
 #include "message_filters/subscriber.h"
@@ -82,6 +83,9 @@ class DisparityNode : public rclcpp::Node
 DisparityNode::DisparityNode(const rclcpp::NodeOptions & options)
 : rclcpp::Node("DisparityNode", options)
 {
+  // TransportHints does not actually declare the parameter
+  this->declare_parameter<std::string>("image_transport", "raw");
+
   // Read parameters
   int queue_size = this->declare_parameter<int>("queue_size", 5);
   min_range_ = this->declare_parameter<double>("min_range", 0.0);
@@ -106,8 +110,12 @@ DisparityNode::DisparityNode(const rclcpp::NodeOptions & options)
         sub_depth_image_.unsubscribe();
         sub_info_.unsubscribe();
       } else if (!sub_depth_image_.getSubscriber()) {
-        image_transport::TransportHints hints(this, "raw");
-        sub_depth_image_.subscribe(this, "left/image_rect", hints.getTransport());
+        // 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("left/image_rect", false);
+        image_transport::TransportHints hints(this);
+        sub_depth_image_.subscribe(this, topic, hints.getTransport());
         sub_info_.subscribe(this, "right/camera_info");
       }
     };

depth_image_proc/src/point_cloud_xyz.cpp

@@ -33,6 +33,7 @@
 #include <functional>
 #include <memory>
 #include <mutex>
+#include <string>
 
 #include "depth_image_proc/visibility.h"
 #include "image_geometry/pinhole_camera_model.hpp"
@@ -51,6 +52,9 @@ namespace depth_image_proc
 PointCloudXyzNode::PointCloudXyzNode(const rclcpp::NodeOptions & options)
 : Node("PointCloudXyzNode", options)
 {
+  // TransportHints does not actually declare the parameter
+  this->declare_parameter<std::string>("depth_image_transport", "raw");
+
   // Read parameters
   queue_size_ = this->declare_parameter<int>("queue_size", 5);
 
@@ -63,16 +67,23 @@ PointCloudXyzNode::PointCloudXyzNode(const rclcpp::NodeOptions & options)
       if (s.current_count == 0) {
         sub_depth_.shutdown();
       } else if (!sub_depth_) {
+        // 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);
+
+        // Get transport and QoS
+        image_transport::TransportHints depth_hints(this, "raw", "depth_image_transport");
         auto custom_qos = rmw_qos_profile_system_default;
         custom_qos.depth = queue_size_;
 
         sub_depth_ = image_transport::create_camera_subscription(
           this,
-          "image_rect",
+          topic,
           std::bind(
             &PointCloudXyzNode::depthCb, this, std::placeholders::_1,
             std::placeholders::_2),
-          "raw",
+          depth_hints.getTransport(),
           custom_qos);
       }
     };

depth_image_proc/src/point_cloud_xyz_radial.cpp

@@ -33,6 +33,7 @@
 #include <functional>
 #include <memory>
 #include <mutex>
+#include <string>
 
 #include "image_geometry/pinhole_camera_model.hpp"
 
@@ -46,10 +47,12 @@
 namespace depth_image_proc
 {
 
-
 PointCloudXyzRadialNode::PointCloudXyzRadialNode(const rclcpp::NodeOptions & options)
 : rclcpp::Node("PointCloudXyzRadialNode", options)
 {
+  // TransportHints does not actually declare the parameter
+  this->declare_parameter<std::string>("depth_image_transport", "raw");
+
   // Read parameters
   queue_size_ = this->declare_parameter<int>("queue_size", 5);
 
@@ -62,16 +65,22 @@ PointCloudXyzRadialNode::PointCloudXyzRadialNode(const rclcpp::NodeOptions & opt
       if (s.current_count == 0) {
         sub_depth_.shutdown();
       } else if (!sub_depth_) {
+        // 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);
+        // Get transport and QoS
+        image_transport::TransportHints depth_hints(this, "raw", "depth_image_transport");
         auto custom_qos = rmw_qos_profile_system_default;
         custom_qos.depth = queue_size_;
-
+        // Create subscriber
         sub_depth_ = image_transport::create_camera_subscription(
           this,
-          "image_raw",
+          topic,
           std::bind(
             &PointCloudXyzRadialNode::depthCb, this, std::placeholders::_1,
             std::placeholders::_2),
-          "raw",
+          depth_hints.getTransport(),
           custom_qos);
       }
     };

depth_image_proc/src/point_cloud_xyzi.cpp

@@ -37,6 +37,7 @@
 
 #include "cv_bridge/cv_bridge.hpp"
 
+#include <image_transport/camera_common.hpp>
 #include <image_transport/image_transport.hpp>
 #include <image_transport/subscriber_filter.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -50,10 +51,13 @@
 namespace depth_image_proc
 {
 
-
 PointCloudXyziNode::PointCloudXyziNode(const rclcpp::NodeOptions & options)
 : rclcpp::Node("PointCloudXyziNode", options)
 {
+  // TransportHints does not actually declare the parameter
+  this->declare_parameter<std::string>("image_transport", "raw");
+  this->declare_parameter<std::string>("depth_image_transport", "raw");
+
   // Read parameters
   int queue_size = this->declare_parameter<int>("queue_size", 5);
 
@@ -82,17 +86,26 @@ PointCloudXyziNode::PointCloudXyziNode(const rclcpp::NodeOptions & options)
         sub_intensity_.unsubscribe();
         sub_info_.unsubscribe();
       } else if (!sub_depth_.getSubscriber()) {
-        // parameter for depth_image_transport hint
-        std::string depth_image_transport_param = "depth_image_transport";
+        // 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 depth_topic =
+          node_base->resolve_topic_or_service_name("depth/image_rect", false);
+        std::string intensity_topic =
+          node_base->resolve_topic_or_service_name("intensity/image_rect", false);
+        // Allow also remapping camera_info to something different than default
+        std::string intensity_info_topic =
+          node_base->resolve_topic_or_service_name(
+          image_transport::getCameraInfoTopic(intensity_topic), false);
 
         // depth image can use different transport.(e.g. compressedDepth)
-        image_transport::TransportHints depth_hints(this, "raw", depth_image_transport_param);
-        sub_depth_.subscribe(this, "depth/image_rect", depth_hints.getTransport());
+        image_transport::TransportHints depth_hints(this, "raw", "depth_image_transport");
+        sub_depth_.subscribe(this, depth_topic, depth_hints.getTransport());
 
         // intensity uses normal ros transport hints.
         image_transport::TransportHints hints(this, "raw");
-        sub_intensity_.subscribe(this, "intensity/image_rect", hints.getTransport());
-        sub_info_.subscribe(this, "intensity/camera_info");
+        sub_intensity_.subscribe(this, intensity_topic, hints.getTransport());
+        sub_info_.subscribe(this, intensity_info_topic);
       }
     };
   pub_point_cloud_ = create_publisher<PointCloud>("points", rclcpp::SensorDataQoS(), pub_options);

depth_image_proc/src/point_cloud_xyzi_radial.cpp

@@ -37,6 +37,7 @@
 
 #include "depth_image_proc/visibility.h"
 
+#include <image_transport/camera_common.hpp>
 #include <image_transport/image_transport.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
@@ -48,10 +49,13 @@
 namespace depth_image_proc
 {
 
-
 PointCloudXyziRadialNode::PointCloudXyziRadialNode(const rclcpp::NodeOptions & options)
 : rclcpp::Node("PointCloudXyziRadialNode", options)
 {
+  // TransportHints does not actually declare the parameter
+  this->declare_parameter<std::string>("image_transport", "raw");
+  this->declare_parameter<std::string>("depth_image_transport", "raw");
+
   // Read parameters
   queue_size_ = this->declare_parameter<int>("queue_size", 5);
 
@@ -80,17 +84,26 @@ PointCloudXyziRadialNode::PointCloudXyziRadialNode(const rclcpp::NodeOptions & o
         sub_intensity_.unsubscribe();
         sub_info_.unsubscribe();
       } else if (!sub_depth_.getSubscriber()) {
-        // parameter for depth_image_transport hint
-        std::string depth_image_transport_param = "depth_image_transport";
+        // 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 depth_topic =
+          node_base->resolve_topic_or_service_name("depth/image_rect", 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
+        std::string intensity_info_topic =
+          node_base->resolve_topic_or_service_name(
+          image_transport::getCameraInfoTopic(intensity_topic), false);
 
         // depth image can use different transport.(e.g. compressedDepth)
-        image_transport::TransportHints depth_hints(this, "raw", depth_image_transport_param);
-        sub_depth_.subscribe(this, "depth/image_raw", depth_hints.getTransport());
+        image_transport::TransportHints depth_hints(this, "raw", "depth_image_transport");
+        sub_depth_.subscribe(this, depth_topic, depth_hints.getTransport());
 
         // intensity uses normal ros transport hints.
-        image_transport::TransportHints hints(this, "raw");
-        sub_intensity_.subscribe(this, "intensity/image_raw", hints.getTransport());
-        sub_info_.subscribe(this, "intensity/camera_info");
+        image_transport::TransportHints hints(this);
+        sub_intensity_.subscribe(this, intensity_topic, hints.getTransport());
+        sub_info_.subscribe(this, intensity_info_topic);
       }
     };
   pub_point_cloud_ = create_publisher<sensor_msgs::msg::PointCloud2>(

depth_image_proc/src/point_cloud_xyzrgb.cpp

@@ -39,6 +39,7 @@
 
 #include <depth_image_proc/conversions.hpp>
 #include <depth_image_proc/point_cloud_xyzrgb.hpp>
+#include <image_transport/camera_common.hpp>
 #include <image_transport/image_transport.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -47,10 +48,13 @@
 namespace depth_image_proc
 {
 
-
 PointCloudXyzrgbNode::PointCloudXyzrgbNode(const rclcpp::NodeOptions & options)
 : rclcpp::Node("PointCloudXyzrgbNode", options)
 {
+  // TransportHints does not actually declare the parameter
+  this->declare_parameter<std::string>("image_transport", "raw");
+  this->declare_parameter<std::string>("depth_image_transport", "raw");
+
   // Read parameters
   int queue_size = this->declare_parameter<int>("queue_size", 5);
   bool use_exact_sync = this->declare_parameter<bool>("exact_sync", false);
@@ -91,9 +95,20 @@ PointCloudXyzrgbNode::PointCloudXyzrgbNode(const rclcpp::NodeOptions & options)
         sub_rgb_.unsubscribe();
         sub_info_.unsubscribe();
       } else if (!sub_depth_.getSubscriber()) {
+        // 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 depth_topic =
+          node_base->resolve_topic_or_service_name("depth_registered/image_rect", false);
+        std::string rgb_topic =
+          node_base->resolve_topic_or_service_name("rgb/image_rect_color", false);
+        // Allow also remapping camera_info to something different than default
+        std::string rgb_info_topic =
+          node_base->resolve_topic_or_service_name(
+          image_transport::getCameraInfoTopic(rgb_topic), false);
+
         // parameter for depth_image_transport hint
-        std::string depth_image_transport_param = "depth_image_transport";
-        image_transport::TransportHints depth_hints(this, "raw", depth_image_transport_param);
+        image_transport::TransportHints depth_hints(this, "raw", "depth_image_transport");
 
         rclcpp::SubscriptionOptions sub_opts;
         // Update the subscription options to allow reconfigurable qos settings.
@@ -108,15 +123,15 @@ PointCloudXyzrgbNode::PointCloudXyzrgbNode(const rclcpp::NodeOptions & options)
 
         // depth image can use different transport.(e.g. compressedDepth)
         sub_depth_.subscribe(
-          this, "depth_registered/image_rect",
+          this, depth_topic,
           depth_hints.getTransport(), rmw_qos_profile_default, sub_opts);
 
         // rgb uses normal ros transport hints.
-        image_transport::TransportHints hints(this, "raw");
+        image_transport::TransportHints hints(this);
         sub_rgb_.subscribe(
-          this, "rgb/image_rect_color",
+          this, rgb_topic,
           hints.getTransport(), rmw_qos_profile_default, sub_opts);
-        sub_info_.subscribe(this, "rgb/camera_info");
+        sub_info_.subscribe(this, rgb_info_topic);
       }
     };
   pub_point_cloud_ = create_publisher<PointCloud2>("points", rclcpp::SensorDataQoS(), pub_options);