From 9f52403a5a7d2491525496256f5bdecdd6e4a1b6 Mon Sep 17 00:00:00 2001
From: Joe Schornak <jschornak@theaiinstitute.com>
Date: Thu, 2 Nov 2023 14:16:11 -0400
Subject: [PATCH] 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 <image_transport/camera_publisher.hpp>
+#include <image_transport/image_transport.hpp>
 #include <rclcpp/node.hpp>
 #include <sensor_msgs/msg/image.hpp>
 #include <spot_driver_cpp/interfaces/publisher_interface_base.hpp>
@@ -42,10 +44,10 @@ class RclcppPublisherInterface : public PublisherInterfaceBase {
   /** @brief rclcpp node use to create the publishers. */
   std::shared_ptr<rclcpp::Node> node_;
 
-  /** @brief Map between image topic names and image publishers. */
-  std::unordered_map<std::string, std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::Image>>> 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<std::string, std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::CameraInfo>>> info_publishers_;
+  /** @brief Map between image topic names and camera publishers. */
+  std::unordered_map<std::string, image_transport::CameraPublisher> 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 <rclcpp/qos.hpp>
 #include <spot_driver_cpp/interfaces/rclcpp_publisher_interface.hpp>
 #include <tl_expected/expected.hpp>
 
 namespace {
 constexpr auto kPublisherHistoryDepth = 1;
-
 constexpr auto kImageTopicSuffix = "image";
-constexpr auto kCameraInfoTopicSuffix = "camera_info";
 }  // namespace
 
 namespace spot_ros2 {
 
-RclcppPublisherInterface::RclcppPublisherInterface(const std::shared_ptr<rclcpp::Node>& node) : node_{node} {}
+RclcppPublisherInterface::RclcppPublisherInterface(const std::shared_ptr<rclcpp::Node>& node)
+    : node_{node}, image_transport_{node_} {}
 
 void RclcppPublisherInterface::createPublishers(const std::set<ImageSource>& 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<sensor_msgs::msg::Image>(
-                                      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<sensor_msgs::msg::CameraInfo>(
-                                     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<void, std::string> RclcppPublisherInterface::publish(
     const std::map<ImageSource, ImageWithCameraInfo>& 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 {};