Merge branch 'optimize/ros2-image-transport-reuse' into v2/develop

orbbec_camera/include/orbbec_camera/image_publisher.h

@@ -49,4 +49,11 @@ class image_transport_publisher : public image_publisher {
  private:
   std::shared_ptr<image_transport::Publisher> image_publisher_impl;
 };
+
+// Keep image_transport plugins and their DDS endpoints alive across camera reconnects.
+std::shared_ptr<image_publisher> getGlobalImageTransportPublisher(rclcpp::Node& node,
+                                                                  const std::string& topic_name,
+                                                                  const rmw_qos_profile_t& qos);
+void releaseGlobalImageTransportPublisher(rclcpp::Node& node, const std::string& topic_name);
+void clearGlobalImageTransportPublishers(rclcpp::Node& node);
 }  // namespace orbbec_camera

orbbec_camera/src/image_publisher.cpp

@@ -14,8 +14,47 @@
 
 #include "orbbec_camera/image_publisher.h"
 
+#include <map>
+#include <mutex>
+#include <utility>
+
 namespace orbbec_camera {
 
+namespace {
+using ImageTransportPublisherCacheKey = std::pair<std::string, std::string>;
+
+struct CachedImageTransportPublisher {
+  rmw_qos_profile_t qos;
+  std::shared_ptr<image_publisher> publisher;
+};
+
+using ImageTransportPublisherCache =
+    std::map<ImageTransportPublisherCacheKey, CachedImageTransportPublisher>;
+
+std::mutex& imageTransportPublisherCacheMutex() {
+  static std::mutex mutex;
+  return mutex;
+}
+
+ImageTransportPublisherCache& imageTransportPublisherCache() {
+  static ImageTransportPublisherCache cache;
+  return cache;
+}
+
+bool rmwTimeEqual(const rmw_time_t& lhs, const rmw_time_t& rhs) {
+  return lhs.sec == rhs.sec && lhs.nsec == rhs.nsec;
+}
+
+bool qosProfilesEqual(const rmw_qos_profile_t& lhs, const rmw_qos_profile_t& rhs) {
+  return lhs.history == rhs.history && lhs.depth == rhs.depth &&
+         lhs.reliability == rhs.reliability && lhs.durability == rhs.durability &&
+         rmwTimeEqual(lhs.deadline, rhs.deadline) && rmwTimeEqual(lhs.lifespan, rhs.lifespan) &&
+         lhs.liveliness == rhs.liveliness &&
+         rmwTimeEqual(lhs.liveliness_lease_duration, rhs.liveliness_lease_duration) &&
+         lhs.avoid_ros_namespace_conventions == rhs.avoid_ros_namespace_conventions;
+}
+}  // namespace
+
 // --- image_rcl_publisher implementation ---
 image_rcl_publisher::image_rcl_publisher(rclcpp::Node& node, const std::string& topic_name,
                                          const rmw_qos_profile_t& qos) {
@@ -57,4 +96,39 @@ void image_transport_publisher::publish(sensor_msgs::msg::Image::UniquePtr image
 size_t image_transport_publisher::get_subscription_count() const {
   return image_publisher_impl->getNumSubscribers();
 }
+
+std::shared_ptr<image_publisher> getGlobalImageTransportPublisher(rclcpp::Node& node,
+                                                                  const std::string& topic_name,
+                                                                  const rmw_qos_profile_t& qos) {
+  const ImageTransportPublisherCacheKey key{node.get_fully_qualified_name(), topic_name};
+  std::lock_guard<std::mutex> lock(imageTransportPublisherCacheMutex());
+  auto& cache = imageTransportPublisherCache();
+  auto cached = cache.find(key);
+  if (cached != cache.end() && qosProfilesEqual(cached->second.qos, qos)) {
+    return cached->second.publisher;
+  }
+
+  auto publisher = std::make_shared<image_transport_publisher>(node, topic_name, qos);
+  cache[key] = CachedImageTransportPublisher{qos, publisher};
+  return publisher;
+}
+
+void releaseGlobalImageTransportPublisher(rclcpp::Node& node, const std::string& topic_name) {
+  const ImageTransportPublisherCacheKey key{node.get_fully_qualified_name(), topic_name};
+  std::lock_guard<std::mutex> lock(imageTransportPublisherCacheMutex());
+  imageTransportPublisherCache().erase(key);
+}
+
+void clearGlobalImageTransportPublishers(rclcpp::Node& node) {
+  const std::string node_name = node.get_fully_qualified_name();
+  std::lock_guard<std::mutex> lock(imageTransportPublisherCacheMutex());
+  auto& cache = imageTransportPublisherCache();
+  for (auto publisher = cache.begin(); publisher != cache.end();) {
+    if (publisher->first.first == node_name) {
+      publisher = cache.erase(publisher);
+    } else {
+      ++publisher;
+    }
+  }
+}
 }  // namespace orbbec_camera

orbbec_camera/src/ob_camera_node.cpp

@@ -5443,26 +5443,27 @@ void OBCameraNode::setupCameraInfo() {
 }
 
 void OBCameraNode::setupImagePublisher(const stream_index_pair &stream_index) {
+  const std::string topic = stream_name_[stream_index] + "/image_raw";
   if (!enable_stream_[stream_index]) {
+    releaseGlobalImageTransportPublisher(*node_, topic);
     image_publishers_.erase(stream_index);
     compressed_image_publishers_.erase(stream_index);
     return;
   }
 
-  const std::string topic = stream_name_[stream_index] + "/image_raw";
   const auto image_qos_profile = getImageQosProfile(stream_index);
   const bool is_mjpg_color_stream =
       (stream_index == COLOR || stream_index == COLOR_LEFT || stream_index == COLOR_RIGHT) &&
       format_[stream_index] == OB_FORMAT_MJPG;
   if (use_intra_process_ || is_mjpg_color_stream) {
+    releaseGlobalImageTransportPublisher(*node_, topic);
     image_publishers_[stream_index] =
         std::make_shared<image_rcl_publisher>(*node_, topic, image_qos_profile);
   } else {
     image_publishers_[stream_index] =
-        std::make_shared<image_transport_publisher>(*node_, topic, image_qos_profile);
+        getGlobalImageTransportPublisher(*node_, topic, image_qos_profile);
   }
-  RCLCPP_INFO_STREAM(logger_,
-                     topic << " QoS: " << getRMWQosProfileDescription(image_qos_profile));
+  RCLCPP_INFO_STREAM(logger_, topic << " QoS: " << getRMWQosProfileDescription(image_qos_profile));
 
   if (is_mjpg_color_stream) {
     compressed_image_publishers_[stream_index] =
@@ -5647,7 +5648,7 @@ void OBCameraNode::syncSoftwareAlignment() {
         depth_unaligned_publisher_ = std::make_shared<image_rcl_publisher>(
             *node_, "depth/image_unaligned", depth_image_qos_profile);
       } else {
-        depth_unaligned_publisher_ = std::make_shared<image_transport_publisher>(
+        depth_unaligned_publisher_ = getGlobalImageTransportPublisher(
             *node_, "depth/image_unaligned", depth_image_qos_profile);
       }
     }
@@ -5655,6 +5656,7 @@ void OBCameraNode::syncSoftwareAlignment() {
   }
 
   align_filter_.reset();
+  releaseGlobalImageTransportPublisher(*node_, "depth/image_unaligned");
   depth_unaligned_publisher_.reset();
 }
 

orbbec_camera/src/ob_camera_node_driver.cpp

@@ -273,6 +273,7 @@ OBCameraNodeDriver::~OBCameraNodeDriver() {
     orb_device_lock_shm_fd_ = -1;
   }
   shm_unlink(ORB_DEFAULT_LOCK_NAME.c_str());
+  clearGlobalImageTransportPublishers(*this);
 }
 
 void OBCameraNodeDriver::init() {