Usage of Image Transport

astra_camera/CMakeLists.txt

@@ -213,6 +213,14 @@ target_link_libraries(clean_shm_node
   ${PROJECT_NAME}
   )
 
+target_compile_definitions(${PROJECT_NAME} PRIVATE "COMPOSITION_BUILDING_DLL")
+
+rclcpp_components_register_nodes(${PROJECT_NAME}
+  "${PROJECT_NAME}::PointCloudXyzrgbNode"
+  "${PROJECT_NAME}::PointCloudXyzNode"
+  "${PROJECT_NAME}::OBCameraNodeFactory")
+
+
 install(TARGETS ${PROJECT_NAME}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib

astra_camera/include/astra_camera/ob_camera_node.h

@@ -45,7 +45,6 @@
 #include "uvc_camera_driver.h"
 #include "dynamic_params.h"
 #include "types.h"
-#include "point_cloud_proc/point_cloud_proc.h"
 #include "magic_enum/magic_enum.hpp"
 
 
@@ -219,8 +218,7 @@ class OBCameraNode {
   std::map<stream_index_pair, openni::VideoMode> stream_video_mode_;
   std::map<stream_index_pair, std::vector<openni::VideoMode>> supported_video_modes_;
   std::map<stream_index_pair, int> unit_step_size_;
-  std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr>
-      image_publishers_;
+  std::map<stream_index_pair, image_transport::Publisher> image_publishers_;
   std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr>
       camera_info_publishers_;
 
@@ -260,8 +258,6 @@ class OBCameraNode {
   ImageROI depth_roi_;
   int depth_scale_ = 1;
   std::string point_cloud_qos_;
-  std::unique_ptr<PointCloudXyzNode> point_cloud_processor_ = nullptr;
-  std::unique_ptr<PointCloudXyzrgbNode> colored_point_cloud_processor_ = nullptr;
   bool enable_point_cloud_ = true;
   bool enable_colored_point_cloud_ = false;
   std::recursive_mutex device_lock_;

astra_camera/include/astra_camera/point_cloud_proc/point_cloud_xyz.h

@@ -45,9 +45,9 @@ namespace astra_camera {
 
 namespace enc = sensor_msgs::image_encodings;
 
-class PointCloudXyzNode {
+class PointCloudXyzNode : public rclcpp::Node {
  public:
-  explicit PointCloudXyzNode(rclcpp::Node* const node, std::shared_ptr<Parameters> parameters);
+  explicit PointCloudXyzNode(const rclcpp::NodeOptions& options);
 
   template <typename T>
   static void convertDepth(const sensor_msgs::msg::Image::ConstSharedPtr& depth_msg,
@@ -58,7 +58,6 @@ class PointCloudXyzNode {
   using PointCloud2 = sensor_msgs::msg::PointCloud2;
   using Image = sensor_msgs::msg::Image;
   using CameraInfo = sensor_msgs::msg::CameraInfo;
-  rclcpp::Node* const node_;
   std::shared_ptr<Parameters> parameters_;
   rclcpp::Logger logger_ = rclcpp::get_logger("PointCloudXyzNode");
   // Subscriptions

astra_camera/include/astra_camera/point_cloud_proc/point_cloud_xyzrgb.h

@@ -59,9 +59,9 @@ namespace astra_camera {
 
 namespace enc = sensor_msgs::image_encodings;
 
-class PointCloudXyzrgbNode {
+class PointCloudXyzrgbNode : public rclcpp::Node{
  public:
-  explicit PointCloudXyzrgbNode(rclcpp::Node* node, std::shared_ptr<Parameters> parameters);
+  explicit PointCloudXyzrgbNode(const rclcpp::NodeOptions& options);
   void convertRgb(const sensor_msgs::msg::Image::ConstSharedPtr& rgb_msg,
                   sensor_msgs::msg::PointCloud2::SharedPtr& cloud_msg, int red_offset,
                   int green_offset, int blue_offset, int color_step);
@@ -70,7 +70,6 @@ class PointCloudXyzrgbNode {
   using PointCloud2 = sensor_msgs::msg::PointCloud2;
   using Image = sensor_msgs::msg::Image;
   using CameraInfo = sensor_msgs::msg::CameraInfo;
-  rclcpp::Node* const node_;
   std::shared_ptr<Parameters> parameters_;
   rmw_qos_profile_t point_cloud_qos_profile_;
   rmw_qos_profile_t color_qos_profile_, depth_qos_profile_, info_qos_profile_;

astra_camera/include/astra_camera/uvc_camera_driver.h

@@ -126,7 +126,7 @@ class UVCCameraDriver {
   std::shared_ptr<Parameters> parameters_;
   UVCCameraConfig config_;
   ImageROI roi_;
-  std::string camera_name_ = "camera";
+  std::string camera_name_ = "head_front_camera";
   std::string color_info_url_;
   uvc_context_t* ctx_ = nullptr;
   uvc_device_t* device_ = nullptr;
@@ -151,7 +151,7 @@ class UVCCameraDriver {
   rclcpp::Service<SetBool>::SharedPtr set_uvc_mirror_srv_;
   rclcpp::Service<SetBool>::SharedPtr toggle_uvc_camera_srv_;
 
-  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_publisher_;
+  image_transport::Publisher image_publisher_;  
   rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr camera_info_publisher_;
   sensor_msgs::msg::CameraInfo camera_info_;
   std::unique_ptr<camera_info_manager::CameraInfoManager> camera_info_manager_ = nullptr;

astra_camera/src/ob_camera_node.cpp

@@ -23,12 +23,6 @@ void OBCameraNode::init() {
   startStreams();
   run_streaming_poller_ = true;
   poll_stream_thread_ = std::make_shared<std::thread>([this]() { pollFrame(); });
-  if (enable_point_cloud_) {
-    point_cloud_processor_ = std::make_unique<PointCloudXyzNode>(node_, parameters_);
-  }
-  if (enable_colored_point_cloud_) {
-    colored_point_cloud_processor_ = std::make_unique<PointCloudXyzrgbNode>(node_, parameters_);
-  }
   is_initialized_ = true;
 }
 
@@ -383,9 +377,7 @@ void OBCameraNode::setupPublishers() {
       std::string topic = name + "/image_raw";
       auto image_qos = image_qos_[stream_index];
       auto image_qos_profile = getRMWQosProfileFromString(image_qos);
-      image_publishers_[stream_index] = node_->create_publisher<sensor_msgs::msg::Image>(
-          topic,
-          rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(image_qos_profile), image_qos_profile));
+      image_publishers_[stream_index] = image_transport::create_publisher(node_, topic, image_qos_profile);
       topic = name + "/camera_info";
       auto camera_info_qos = camera_info_qos_[stream_index];
       auto camera_info_qos_profile = getRMWQosProfileFromString(camera_info_qos);
@@ -531,7 +523,7 @@ void OBCameraNode::onNewFrameCallback(const openni::VideoFrameRef& frame,
   image_msg->height = stream_index == DEPTH ? height * depth_scale_ : height;
   image_msg->step = image_msg->width * unit_step_size_[stream_index];
   image_msg->is_bigendian = false;
-  image_publisher->publish(*image_msg);
+  image_publisher.publish(image_msg);
   sensor_msgs::msg::CameraInfo camera_info;
   if (stream_index == DEPTH) {
     camera_info = getDepthCameraInfo();

astra_camera/src/point_cloud_proc/point_cloud_xyz.cpp

@@ -39,10 +39,10 @@
 
 namespace astra_camera {
 
-PointCloudXyzNode::PointCloudXyzNode(rclcpp::Node *const node,
-                                     std::shared_ptr<Parameters> parameters)
-    : node_(node), parameters_(std::move(parameters)) {
+PointCloudXyzNode::PointCloudXyzNode(const rclcpp::NodeOptions& options)
+    : Node("PointCloudXyzNode", options){
   // Read parameters
+  parameters_ = std::make_shared<Parameters>(this);    
   setAndGetNodeParameter<int>(parameters_, queue_size_, "queue_size", 5);
   std::string point_cloud_qos;
   std::string depth_qos;
@@ -59,7 +59,7 @@ PointCloudXyzNode::PointCloudXyzNode(rclcpp::Node *const node,
   std::scoped_lock<decltype(connect_mutex_)> lock(connect_mutex_);
   // TODO(ros2) Implement when SubscriberStatusCallback is available
   point_cloud_qos_profile_ = getRMWQosProfileFromString(point_cloud_qos);
-  pub_point_cloud_ = node_->create_publisher<PointCloud2>(
+  pub_point_cloud_ = this->create_publisher<PointCloud2>(
       "depth/points", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(point_cloud_qos_profile_),
                                   point_cloud_qos_profile_));
 }
@@ -114,7 +114,7 @@ void PointCloudXyzNode::connectCb() {
     custom_qos.depth = queue_size_;
 
     sub_depth_ = image_transport::create_camera_subscription(
-        node_, "depth/image_raw",
+        this, "depth/image_raw",
         [this](const sensor_msgs::msg::Image::ConstSharedPtr &msg,
                const sensor_msgs::msg::CameraInfo::ConstSharedPtr &info) { depthCb(msg, info); },
         "raw", custom_qos);
@@ -150,3 +150,10 @@ void PointCloudXyzNode::depthCb(const Image::ConstSharedPtr &depth_msg,
 }
 
 }  // namespace astra_camera
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+RCLCPP_COMPONENTS_REGISTER_NODE(astra_camera::PointCloudXyzNode)
+
+

astra_camera/src/point_cloud_proc/point_cloud_xyzrgb.cpp

@@ -43,16 +43,13 @@
 #include <cv_bridge/cv_bridge.h>
 #include <opencv2/imgproc/imgproc.hpp>
 
-#include <memory>
-#include <string>
-#include <vector>
 #include "astra_camera/point_cloud_proc/point_cloud_xyzrgb.h"
 namespace astra_camera {
 
-PointCloudXyzrgbNode::PointCloudXyzrgbNode(rclcpp::Node* const node,
-                                           std::shared_ptr<Parameters> parameters)
-    : node_(node), parameters_(std::move(parameters)) {
+PointCloudXyzrgbNode::PointCloudXyzrgbNode(const rclcpp::NodeOptions& options)
+    : Node("PointCloudXyzrgbNode", options) {
   // Read parameters
+  parameters_ = std::make_shared<Parameters>(this);    
   int queue_size = 5;
   bool use_exact_sync = false;
   setAndGetNodeParameter<int>(parameters_, queue_size, "queue_size", 5);
@@ -90,7 +87,7 @@ PointCloudXyzrgbNode::PointCloudXyzrgbNode(rclcpp::Node* const node,
   // Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_
   std::lock_guard<std::mutex> lock(connect_mutex_);
   // TODO(ros2) Implement connect_cb when SubscriberStatusCallback is available
-  pub_point_cloud_ = node_->create_publisher<PointCloud2>(
+  pub_point_cloud_ = this->create_publisher<PointCloud2>(
       "depth/color/points",
       rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(point_cloud_qos_profile_),
                   point_cloud_qos_profile_));
@@ -121,15 +118,15 @@ void PointCloudXyzrgbNode::connectCb() {
   // TODO(ros2) Implement getNumSubscribers when rcl/rmw support it
   // parameter for depth_image_transport hint
   std::string depth_image_transport_param = "depth_image_transport";
-  image_transport::TransportHints depth_hints(node_, "raw", depth_image_transport_param);
+  image_transport::TransportHints depth_hints(this, "raw", depth_image_transport_param);
 
   // depth image can use different transport.(e.g. compressedDepth)
-  sub_depth_.subscribe(node_, "depth/image_raw", depth_hints.getTransport(), depth_qos_profile_);
+  sub_depth_.subscribe(this, "depth/image_raw", depth_hints.getTransport(), depth_qos_profile_);
 
   // rgb uses normal ros transport hints.
-  image_transport::TransportHints hints(node_, "raw");
-  sub_rgb_.subscribe(node_, "color/image_raw", hints.getTransport(), color_qos_profile_);
-  sub_info_.subscribe(node_, "color/camera_info", info_qos_profile_);
+  image_transport::TransportHints hints(this, "raw");
+  sub_rgb_.subscribe(this, "color/image_raw", hints.getTransport(), color_qos_profile_);
+  sub_info_.subscribe(this, "color/camera_info", info_qos_profile_);
 }
 
 void PointCloudXyzrgbNode::imageCb(const Image::ConstSharedPtr& depth_msg,
@@ -186,7 +183,7 @@ void PointCloudXyzrgbNode::imageCb(const Image::ConstSharedPtr& depth_msg,
       rgb_msg = cv_bridge::toCvCopy(cv_rsz.toImageMsg(), enc::RGB8)->toImageMsg();
     }
 
-    RCLCPP_ERROR_THROTTLE(logger_, *(node_->get_clock()), 50000,
+    RCLCPP_ERROR_THROTTLE(logger_, *(this->get_clock()), 50000,
                           "Depth resolution (%ux%u) does not match RGB resolution (%ux%u)",
                           depth_msg->width, depth_msg->height, rgb_msg->width, rgb_msg->height);
     return;
@@ -260,3 +257,8 @@ void PointCloudXyzrgbNode::imageCb(const Image::ConstSharedPtr& depth_msg,
 }
 
 }  // namespace astra_camera
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+RCLCPP_COMPONENTS_REGISTER_NODE(astra_camera::PointCloudXyzrgbNode)

astra_camera/src/uvc_camera_driver.cpp

@@ -87,9 +87,7 @@ UVCCameraDriver::UVCCameraDriver(rclcpp::Node* node, std::shared_ptr<Parameters>
     camera_info_manager_ = std::make_unique<camera_info_manager::CameraInfoManager>(
         node_, "rgb_camera", color_info_url_);
   }
-  image_publisher_ = node_->create_publisher<sensor_msgs::msg::Image>(
-      "color/image_raw",
-      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(color_qos_profile_), color_qos_profile_));
+  image_publisher_ = image_transport::create_publisher(node_, "color/image_raw", color_qos_profile_);
   camera_info_publisher_ = node_->create_publisher<sensor_msgs::msg::CameraInfo>(
       "color/camera_info", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(color_info_qos_profile_),
                                        color_info_qos_profile_));
@@ -395,7 +393,7 @@ void UVCCameraDriver::frameCallback(uvc_frame_t* frame) {
     cv_image_ptr->image = cv_img;
     image = *(cv_image_ptr->toImageMsg());
   }
-  image_publisher_->publish(image);
+  image_publisher_.publish(image);
   auto camera_info = getCameraInfo();
   camera_info.header.stamp = image.header.stamp;
   camera_info.header.frame_id = image.header.frame_id;