diff --git a/orbbec_camera/include/orbbec_camera/image_publisher.h b/orbbec_camera/include/orbbec_camera/image_publisher.h index f114ec1d..32131d58 100644 --- a/orbbec_camera/include/orbbec_camera/image_publisher.h +++ b/orbbec_camera/include/orbbec_camera/image_publisher.h @@ -49,4 +49,11 @@ class image_transport_publisher : public image_publisher { private: std::shared_ptr image_publisher_impl; }; + +// Keep image_transport plugins and their DDS endpoints alive across camera reconnects. +std::shared_ptr 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 diff --git a/orbbec_camera/src/image_publisher.cpp b/orbbec_camera/src/image_publisher.cpp index 0998fedb..a207320e 100644 --- a/orbbec_camera/src/image_publisher.cpp +++ b/orbbec_camera/src/image_publisher.cpp @@ -14,8 +14,47 @@ #include "orbbec_camera/image_publisher.h" +#include +#include +#include + namespace orbbec_camera { +namespace { +using ImageTransportPublisherCacheKey = std::pair; + +struct CachedImageTransportPublisher { + rmw_qos_profile_t qos; + std::shared_ptr publisher; +}; + +using ImageTransportPublisherCache = + std::map; + +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 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 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(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 lock(imageTransportPublisherCacheMutex()); + imageTransportPublisherCache().erase(key); +} + +void clearGlobalImageTransportPublishers(rclcpp::Node& node) { + const std::string node_name = node.get_fully_qualified_name(); + std::lock_guard 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 diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index b4874abe..9d3b72c8 100755 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -5350,13 +5350,14 @@ 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"; auto image_qos_profile = getRMWQosProfileFromString(image_qos_[stream_index]); if (use_intra_process_) { image_qos_profile = rmw_qos_profile_default; @@ -5366,11 +5367,12 @@ void OBCameraNode::setupImagePublisher(const stream_index_pair &stream_index) { (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(*node_, topic, image_qos_profile); } else { image_publishers_[stream_index] = - std::make_shared(*node_, topic, image_qos_profile); + getGlobalImageTransportPublisher(*node_, topic, image_qos_profile); } if (is_mjpg_color_stream) { @@ -5535,7 +5537,7 @@ void OBCameraNode::syncSoftwareAlignment() { depth_unaligned_publisher_ = std::make_shared( *node_, "depth/image_unaligned", depth_image_qos_profile); } else { - depth_unaligned_publisher_ = std::make_shared( + depth_unaligned_publisher_ = getGlobalImageTransportPublisher( *node_, "depth/image_unaligned", depth_image_qos_profile); } } @@ -5543,6 +5545,7 @@ void OBCameraNode::syncSoftwareAlignment() { } align_filter_.reset(); + releaseGlobalImageTransportPublisher(*node_, "depth/image_unaligned"); depth_unaligned_publisher_.reset(); } diff --git a/orbbec_camera/src/ob_camera_node_driver.cpp b/orbbec_camera/src/ob_camera_node_driver.cpp index 9afb2aec..b18c1df1 100644 --- a/orbbec_camera/src/ob_camera_node_driver.cpp +++ b/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() {