Support IPC

image_transport/CMakeLists.txt

@@ -111,6 +111,11 @@ if(BUILD_TESTING)
     target_link_libraries(${PROJECT_NAME}-message_passing ${PROJECT_NAME})
   endif()
 
+  ament_add_gtest(${PROJECT_NAME}-ipc test/test_ipc.cpp)
+  if(TARGET ${PROJECT_NAME}-ipc)
+    target_link_libraries(${PROJECT_NAME}-ipc ${PROJECT_NAME})
+  endif()
+
   ament_add_gtest(${PROJECT_NAME}-remapping test/test_remapping.cpp)
   if(TARGET ${PROJECT_NAME}-remapping)
     target_link_libraries(${PROJECT_NAME}-remapping ${PROJECT_NAME})

image_transport/include/image_transport/camera_publisher.hpp

@@ -115,6 +115,14 @@ class CameraPublisher
     const sensor_msgs::msg::Image::ConstSharedPtr & image,
     const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info) const;
 
+  /*!
+   * \brief Publish an (image, info) pair on the topics associated with this CameraPublisher.
+   */
+  IMAGE_TRANSPORT_PUBLIC
+  void publishUnique(
+    sensor_msgs::msg::Image::UniquePtr image,
+    sensor_msgs::msg::CameraInfo::UniquePtr info) const;
+
   /*!
    * \brief Publish an (image, info) pair with given timestamp on the topics associated with
    * this CameraPublisher.

image_transport/include/image_transport/publisher.hpp

@@ -107,6 +107,12 @@ class Publisher
   IMAGE_TRANSPORT_PUBLIC
   void publish(const sensor_msgs::msg::Image::ConstSharedPtr & message) const;
 
+  /*!
+   * \brief Publish an image on the topics associated with this Publisher.
+   */
+  IMAGE_TRANSPORT_PUBLIC
+  void publishUnique(sensor_msgs::msg::Image::UniquePtr message) const;
+
   /*!
    * \brief Shutdown the advertisements associated with this Publisher.
    */

image_transport/include/image_transport/publisher_plugin.hpp

@@ -89,6 +89,11 @@ class PublisherPlugin
    */
   virtual void publish(const sensor_msgs::msg::Image & message) const = 0;
 
+  /**
+   * \brief Publish an image using the transport associated with this PublisherPlugin.
+   */
+  virtual void publishUnique(sensor_msgs::msg::Image::UniquePtr message) const = 0;
+
   /**
    * \brief Publish an image using the transport associated with this PublisherPlugin.
    */

image_transport/include/image_transport/raw_publisher.hpp

@@ -65,6 +65,11 @@ class RawPublisher : public SimplePublisherPlugin<sensor_msgs::msg::Image>
     publish_fn(message);
   }
 
+  virtual void publishUnique(sensor_msgs::msg::Image::UniquePtr message, const UniqueFn& publish_fn) const
+  {
+    publish_fn(std::move(message));
+  }
+
   virtual std::string getTopicToAdvertise(const std::string& base_topic) const
   {
     return base_topic;

image_transport/include/image_transport/simple_publisher_plugin.hpp

@@ -95,6 +95,17 @@ class SimplePublisherPlugin : public PublisherPlugin
     publish(message, bindInternalPublisher(simple_impl_->pub_.get()));
   }
 
+  virtual void publishUnique(sensor_msgs::msg::Image::UniquePtr message) const
+  {
+    if (!simple_impl_ || !simple_impl_->pub_) {
+      RCLCPP_ERROR(simple_impl_->logger_,
+        "Call to publish() on an invalid image_transport::SimplePublisherPlugin");
+      return;
+    }
+
+    publishUnique(std::move(message), bindUniquePublisher(simple_impl_->pub_.get()));
+  }
+
   virtual void shutdown()
   {
     //if (simple_impl_) simple_impl_->pub_.shutdown();
@@ -116,6 +127,9 @@ class SimplePublisherPlugin : public PublisherPlugin
   //! Generic function for publishing the internal message type.
   typedef std::function<void (const M &)> PublishFn;
 
+  //! Generic function for std::unique_ptr<M>.
+  typedef std::function<void (std::unique_ptr<M>)> UniqueFn;
+
   /**
    * \brief Publish an image using the specified publish function. Must be implemented by
    * the subclass.
@@ -128,6 +142,18 @@ class SimplePublisherPlugin : public PublisherPlugin
     const sensor_msgs::msg::Image & message,
     const PublishFn & publish_fn) const = 0;
 
+  /**
+   * \brief Publish an image using the specified publish function. Must be implemented by
+   * the subclass.
+   *
+   * The UniqueFn publishes the transport-specific message type. This indirection allows
+   * SimpleSubscriberPlugin to use this function for both normal broadcast publishing and
+   * single subscriber publishing (in subscription callbacks).
+   */
+  virtual void publishUnique(
+    sensor_msgs::msg::Image::UniquePtr message,
+    const UniqueFn & publish_fn) const = 0;
+
   /**
    * \brief Return the communication topic name for a given base topic.
    *
@@ -170,6 +196,15 @@ class SimplePublisherPlugin : public PublisherPlugin
     InternalPublishMemFn internal_pub_mem_fn = &PubT::publish;
     return std::bind(internal_pub_mem_fn, pub, std::placeholders::_1);
   }
+
+  template<class PubT>
+  UniqueFn bindUniquePublisher(PubT * pub) const
+  {
+    // Bind PubT::publish(Message::UniquePtr) as UniqueFn
+    typedef void (PubT::* InternalPublishMemFn)(std::unique_ptr<M>);
+    InternalPublishMemFn internal_pub_mem_fn = &PubT::publish;
+    return std::bind(internal_pub_mem_fn, pub, std::placeholders::_1);
+  }
 };
 
 }  // namespace image_transport

image_transport/src/camera_publisher.cpp

@@ -146,6 +146,21 @@ void CameraPublisher::publish(
   impl_->info_pub_->publish(*info);
 }
 
+void CameraPublisher::publishUnique(
+  sensor_msgs::msg::Image::UniquePtr image,
+  sensor_msgs::msg::CameraInfo::UniquePtr info) const
+{
+  if (!impl_ || !impl_->isValid()) {
+    // TODO(ros2) Switch to RCUTILS_ASSERT when ros2/rcutils#112 is merged
+    RCLCPP_FATAL(impl_->logger_,
+      "Call to publish() on an invalid image_transport::CameraPublisher");
+    return;
+  }
+
+  impl_->image_pub_.publishUnique(std::move(image));
+  impl_->info_pub_->publish(std::move(info));
+}
+
 void CameraPublisher::shutdown()
 {
   if (impl_) {

image_transport/src/publisher.cpp

@@ -181,6 +181,21 @@ void Publisher::publish(const sensor_msgs::msg::Image::ConstSharedPtr & message)
   }
 }
 
+void Publisher::publishUnique(sensor_msgs::msg::Image::UniquePtr message) const
+{
+  if (!impl_ || !impl_->isValid()) {
+    // TODO(ros2) Switch to RCUTILS_ASSERT when ros2/rcutils#112 is merged
+    RCLCPP_FATAL(impl_->logger_, "Call to publish() on an invalid image_transport::Publisher");
+    return;
+  }
+
+  for (const auto & pub: impl_->publishers_) {
+    if (pub->getNumSubscribers() > 0) {
+      pub->publishUnique(std::move(message));
+    }
+  }
+}
+
 void Publisher::shutdown()
 {
   if (impl_) {

image_transport/test/test_ipc.cpp

@@ -0,0 +1,121 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2021 Open Robotics
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include <chrono>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/node.hpp"
+
+#include <gtest/gtest.h>
+
+#include "image_transport/image_transport.hpp"
+#include "sensor_msgs/msg/image.hpp"
+
+#include "utils.hpp"
+
+using namespace std::chrono_literals;
+
+class IPCTesting : public ::testing::Test
+{
+protected:
+  void SetUp() override
+  {
+    rclcpp::NodeOptions options{};
+    options.use_intra_process_comms(true);
+    node_ = rclcpp::Node::make_shared("ipc_testing", options);
+  }
+
+  rclcpp::Node::SharedPtr node_;
+  int count_ = 0;
+  std::uintptr_t recv_image_addr_ = 0;
+  std::uintptr_t recv_info_addr_ = 0;
+};
+
+TEST_F(IPCTesting, camera_ipc)
+{
+  const size_t max_retries = 3;
+  const size_t max_loops = 200;
+  const std::chrono::milliseconds sleep_per_loop = std::chrono::milliseconds(10);
+
+  rclcpp::executors::SingleThreadedExecutor executor;
+
+  auto pub = image_transport::create_camera_publisher(node_.get(), "camera/image");
+  auto sub = image_transport::create_camera_subscription(node_.get(), "camera/image",
+    [this](const sensor_msgs::msg::Image::ConstSharedPtr& image,
+      const sensor_msgs::msg::CameraInfo::ConstSharedPtr& info)
+      {
+        count_++;
+        recv_image_addr_ = reinterpret_cast<std::uintptr_t>(image.get());
+        recv_info_addr_ = reinterpret_cast<std::uintptr_t>(info.get());
+      },
+      "raw"
+    );
+
+  test_rclcpp::wait_for_subscriber(node_, sub.getTopic());
+  ASSERT_EQ(0, count_);
+
+  executor.spin_node_some(node_);
+  ASSERT_EQ(0, count_);
+
+  size_t retry = 0;
+  std::uintptr_t send_image_addr;
+  std::uintptr_t send_info_addr;
+  while(retry < max_retries && count_ == 0) {
+    auto image = std::make_unique<sensor_msgs::msg::Image>();
+    auto info = std::make_unique<sensor_msgs::msg::CameraInfo>();
+    send_image_addr = reinterpret_cast<std::uintptr_t>(image.get());
+    send_info_addr = reinterpret_cast<std::uintptr_t>(info.get());
+    pub.publishUnique(std::move(image), std::move(info));
+
+    executor.spin_node_some(node_);
+    size_t loop = 0;
+    while ((count_ != 1) && (loop++ < max_loops)) {
+      std::this_thread::sleep_for(sleep_per_loop);
+      executor.spin_node_some(node_);
+    }
+  }
+
+  EXPECT_EQ(1, count_);
+  EXPECT_EQ(send_image_addr, recv_image_addr_);
+  EXPECT_EQ(send_info_addr, recv_info_addr_);
+}
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  testing::InitGoogleTest(&argc, argv);
+  int ret = RUN_ALL_TESTS();
+  rclcpp::shutdown();
+  return ret;
+}