From ae80926b41fbe394b3425a03be6392d8791ec6fb Mon Sep 17 00:00:00 2001 From: Joe Schornak Date: Thu, 2 Nov 2023 14:15:13 -0400 Subject: [PATCH 1/2] add dependency on image_transport package --- spot_driver_cpp/CMakeLists.txt | 1 + spot_driver_cpp/package.xml | 1 + 2 files changed, 2 insertions(+) diff --git a/spot_driver_cpp/CMakeLists.txt b/spot_driver_cpp/CMakeLists.txt index 04aca1a50..5b66b3270 100644 --- a/spot_driver_cpp/CMakeLists.txt +++ b/spot_driver_cpp/CMakeLists.txt @@ -13,6 +13,7 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS ON) set(THIS_PACKAGE_INCLUDE_ROS_DEPENDS cv_bridge geometry_msgs + image_transport rclcpp rclcpp_components sensor_msgs diff --git a/spot_driver_cpp/package.xml b/spot_driver_cpp/package.xml index a8add7bbb..b0bf76f95 100644 --- a/spot_driver_cpp/package.xml +++ b/spot_driver_cpp/package.xml @@ -10,6 +10,7 @@ ament_cmake bosdyn + image_transport rclcpp rclcpp_components geometry_msgs From 46ddf339b1fc0b49d08a95d6c7f0f30ebbdffcdb Mon Sep 17 00:00:00 2001 From: Joe Schornak Date: Thu, 2 Nov 2023 14:16:11 -0400 Subject: [PATCH 2/2] replace separate Image and CameraInfo publishers with combined image_transport::CameraPublisher --- .../interfaces/rclcpp_publisher_interface.hpp | 10 +++--- .../interfaces/rclcpp_publisher_interface.cpp | 35 +++++-------------- 2 files changed, 14 insertions(+), 31 deletions(-) diff --git a/spot_driver_cpp/include/spot_driver_cpp/interfaces/rclcpp_publisher_interface.hpp b/spot_driver_cpp/include/spot_driver_cpp/interfaces/rclcpp_publisher_interface.hpp index bb4ada43e..26d1c6bdd 100644 --- a/spot_driver_cpp/include/spot_driver_cpp/interfaces/rclcpp_publisher_interface.hpp +++ b/spot_driver_cpp/include/spot_driver_cpp/interfaces/rclcpp_publisher_interface.hpp @@ -2,6 +2,8 @@ #pragma once +#include +#include #include #include #include @@ -42,10 +44,10 @@ class RclcppPublisherInterface : public PublisherInterfaceBase { /** @brief rclcpp node use to create the publishers. */ std::shared_ptr node_; - /** @brief Map between image topic names and image publishers. */ - std::unordered_map>> image_publishers_; + /** @brief ImageTransport instance used to create CameraPublishers. */ + image_transport::ImageTransport image_transport_; - /** @brief Map between camera info topic names and camera info publishers. */ - std::unordered_map>> info_publishers_; + /** @brief Map between image topic names and camera publishers. */ + std::unordered_map publishers_; }; } // namespace spot_ros2 diff --git a/spot_driver_cpp/src/interfaces/rclcpp_publisher_interface.cpp b/spot_driver_cpp/src/interfaces/rclcpp_publisher_interface.cpp index e780253e6..bd0ec99e7 100644 --- a/spot_driver_cpp/src/interfaces/rclcpp_publisher_interface.cpp +++ b/spot_driver_cpp/src/interfaces/rclcpp_publisher_interface.cpp @@ -1,60 +1,41 @@ // Copyright (c) 2023 Boston Dynamics AI Institute LLC. All rights reserved. -#include #include #include namespace { constexpr auto kPublisherHistoryDepth = 1; - constexpr auto kImageTopicSuffix = "image"; -constexpr auto kCameraInfoTopicSuffix = "camera_info"; } // namespace namespace spot_ros2 { -RclcppPublisherInterface::RclcppPublisherInterface(const std::shared_ptr& node) : node_{node} {} +RclcppPublisherInterface::RclcppPublisherInterface(const std::shared_ptr& node) + : node_{node}, image_transport_{node_} {} void RclcppPublisherInterface::createPublishers(const std::set& image_sources) { - image_publishers_.clear(); - info_publishers_.clear(); + publishers_.clear(); for (const auto& image_source : image_sources) { // Since these topic names do not have a leading `/` character, they will be published within the namespace of the // node, which should match the name of the robot. For example, the topic for the front left RGB camera will // ultimately appear as `/MyRobotName/camera/frontleft/image`. - const auto topic_name_base = toRosTopic(image_source); - - const auto image_topic_name = topic_name_base + "/" + kImageTopicSuffix; - - image_publishers_.try_emplace(image_topic_name, - node_->create_publisher( - image_topic_name, rclcpp::QoS(rclcpp::KeepLast(kPublisherHistoryDepth)))); - - const auto info_topic_name = topic_name_base + "/" + kCameraInfoTopicSuffix; - info_publishers_.try_emplace(info_topic_name, - node_->create_publisher( - info_topic_name, rclcpp::QoS(rclcpp::KeepLast(kPublisherHistoryDepth)))); + const auto image_topic_name = toRosTopic(image_source) + "/" + kImageTopicSuffix; + publishers_.try_emplace(image_topic_name, + image_transport_.advertiseCamera(image_topic_name, kPublisherHistoryDepth)); } } tl::expected RclcppPublisherInterface::publish( const std::map& images) { for (const auto& [image_source, image_data] : images) { - const auto topic_name_base = toRosTopic(image_source); - const auto image_topic_name = topic_name_base + "/" + kImageTopicSuffix; - const auto info_topic_name = topic_name_base + "/" + kCameraInfoTopicSuffix; + const auto image_topic_name = toRosTopic(image_source) + "/" + kImageTopicSuffix; try { - image_publishers_.at(image_topic_name)->publish(image_data.image); + publishers_.at(image_topic_name).publish(image_data.image, image_data.info); } catch (const std::out_of_range& e) { return tl::make_unexpected("No publisher exists for image topic `" + image_topic_name + "`."); } - try { - info_publishers_.at(info_topic_name)->publish(image_data.info); - } catch (const std::out_of_range& e) { - return tl::make_unexpected("No publisher exists for camera info topic`" + info_topic_name + "`."); - } } return {};