Implement simple subscriber and publisher.

ros-perception · Jul 27, 2018 · 637689d · 637689d
1 parent aa19a71
commit 637689d
Show file tree

Hide file tree

Showing 16 changed files with 191 additions and 519 deletions.
diff --git a/image_transport/CMakeLists.txt b/image_transport/CMakeLists.txt
@@ -20,6 +20,7 @@ find_package(pluginlib REQUIRED)
 find_package(tinyxml_vendor REQUIRED)
 find_package(TinyXML REQUIRED)
 
+
 include_directories(include)
 
 # Build libimage_transport
@@ -40,8 +41,14 @@ ament_target_dependencies(${PROJECT_NAME}
   "sensor_msgs"
 )
 
+add_library(${PROJECT_NAME}_plugins
+  src/manifest.cpp
+  src/raw_publisher.cpp)
+target_link_libraries(${PROJECT_NAME}_plugins ${PROJECT_NAME})
+pluginlib_export_plugin_description_file(image_transport default_plugins.xml)
+
 install(
-  TARGETS ${PROJECT_NAME}
+  TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_plugins
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
@@ -83,41 +90,3 @@ if(BUILD_TESTING)
     target_link_libraries(${PROJECT_NAME}-single_subscriber_publisher ${PROJECT_NAME})
   endif()
 endif()
-
-
-#add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
-#target_link_libraries(${PROJECT_NAME}
-#  ${Boost_LIBRARIES}
-#  ${catkin_LIBRARIES}
-#)
-
-# Build libimage_transport_plugins
-#add_library(${PROJECT_NAME}_plugins src/manifest.cpp src/raw_publisher.cpp)
-#target_link_libraries(${PROJECT_NAME}_plugins ${PROJECT_NAME})
-#
-#install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_plugins
-#        DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#        COMPONENT main
-#)
-#install(DIRECTORY include/${PROJECT_NAME}/
-#        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#)
-
-# add two execs
-#add_executable(republish src/republish.cpp)
-#target_link_libraries(republish ${PROJECT_NAME})
-
-#add_executable(list_transports src/list_transports.cpp)
-#target_link_libraries(list_transports
-#  ${PROJECT_NAME}
-#  ${catkin_LIBRARIES}
-#)
-
-#install(TARGETS list_transports republish
-#        DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-#)
-
-# add xml file
-#install(FILES default_plugins.xml
-#        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-#)
diff --git a/image_transport/include/image_transport/camera_subscriber.h b/image_transport/include/image_transport/camera_subscriber.h
@@ -35,6 +35,10 @@
 #ifndef IMAGE_TRANSPORT_CAMERA_SUBSCRIBER_H
 #define IMAGE_TRANSPORT_CAMERA_SUBSCRIBER_H
 
+#include <functional>
+
+#include <rclcpp/node.hpp>
+
 #include <sensor_msgs/msg/camera_info.hpp>
 #include <sensor_msgs/msg/image.hpp>
 
@@ -97,13 +101,9 @@ class CameraSubscriber
 
 private:
   CameraSubscriber(ImageTransport& image_it, rclcpp::Node::SharedPtr& info_nh,
-                   const std::string& base_topic, uint32_t queue_size,
-                   const Callback& callback);
-                   /*
-                    * TODO(ros2) implement transport hints and subscriber tracking.
-                   const std::shared_ptr<void>& tracked_object = std::shared_ptr<void>(),
-                   const TransportHints& transport_hints = TransportHints());
-                   */
+                   const std::string& base_topic,
+                   const Callback& callback,
+                   rmw_qos_profile_t = rmw_qos_profile_default);
 
   struct Impl;
   typedef std::shared_ptr<Impl> ImplPtr;

diff --git a/image_transport/include/image_transport/image_transport.h b/image_transport/include/image_transport/image_transport.h
@@ -40,7 +40,8 @@
 #include "image_transport/camera_publisher.h"
 #include "image_transport/camera_subscriber.h"
 
-namespace image_transport {
+namespace image_transport
+{
 
 /**
  * \brief Advertise and subscribe to image topics.
@@ -51,159 +52,94 @@ namespace image_transport {
 class ImageTransport
 {
 public:
-  explicit ImageTransport(const rclcpp::Node::SharedPtr& node);
+  explicit ImageTransport(const rclcpp::Node::SharedPtr & node);
 
   ~ImageTransport();
 
   /*!
    * \brief Advertise an image topic, simple version.
    */
-  Publisher advertise(const std::string& base_topic, rmw_qos_profile_t custom_qos);
-
-  /*!
-   * \brief Advertise an image topic with subcriber status callbacks.
-   */
-  // TODO(ros2) subscriber status callbacks
-  /*
-  Publisher advertise(const std::string& base_topic, uint32_t queue_size,
-                      const SubscriberStatusCallback& connect_cb,
-                      const SubscriberStatusCallback& disconnect_cb = SubscriberStatusCallback(),
-                      const std::shared_ptr<void>& tracked_object = std::shared_ptr<void>(), bool latch = false);
-                      */
+  Publisher advertise(
+    const std::string & base_topic,
+    rmw_qos_profile_t custom_qos = rmw_qos_profile_default);
 
   /**
    * \brief Subscribe to an image topic, version for arbitrary std::function object.
    */
-  Subscriber subscribe(const std::string& base_topic,
-                       const std::function<void(const sensor_msgs::msg::Image::ConstSharedPtr&)>& callback,
-                       rmw_qos_profile_t custom_qos);
+  Subscriber subscribe(
+    const std::string & base_topic,
+    const std::function<void(const sensor_msgs::msg::Image::ConstSharedPtr &)> & callback,
+    rmw_qos_profile_t custom_qos = rmw_qos_profile_default);
 
   /**
    * \brief Subscribe to an image topic, version for bare function.
    */
-  Subscriber subscribe(const std::string& base_topic
-                       void(*fp)(const sensor_msgs::msg::Image::ConstSharedPtr&),
-                        rmw_qos_profile_t custom_qos
-
+  Subscriber subscribe(
+    const std::string & base_topic,
+    void (*fp)(const sensor_msgs::msg::Image::ConstSharedPtr &),
+    rmw_qos_profile_t custom_qos = rmw_qos_profile_default)
   {
-    return subscribe(base_topic, queue_size,
-                     std::function<void(const sensor_msgs::msg::Image::ConstSharedPtr&)>(fp), custom_qos);
+    return subscribe(base_topic,
+             std::function<void(const sensor_msgs::msg::Image::ConstSharedPtr &)>(fp), custom_qos);
   }
 
   /**
    * \brief Subscribe to an image topic, version for class member function with bare pointer.
    */
   template<class T>
-  Subscriber subscribe(const std::string& base_topic, uint32_t queue_size,
-                       void(T::*fp)(const sensor_msgs::msg::Image::ConstSharedPtr&), T* obj)
-                       /*
-                        * TODO(ros2) transport hints
-                       const TransportHints& transport_hints = TransportHints())
-                       */
-  {
-    return subscribe(base_topic, queue_size, std::bind(fp, obj, std::placeholders::_1)); /*std::shared_ptr<void>(), transport_hints);*/
-  }
-
-  /**
-   * \brief Subscribe to an image topic, version for class member function with shared_ptr.
-   */
-  template<class T>
-  Subscriber subscribe(const std::string& base_topic, uint32_t queue_size,
-                       void(T::*fp)(const sensor_msgs::msg::Image::ConstSharedPtr&),
-                       const std::shared_ptr<T>& obj)
-                       /*
-                        * TODO(ros2) transport hints
-                       const TransportHints& transport_hints = TransportHints())
-                       */
+  Subscriber subscribe(
+    const std::string & base_topic,
+    void (T::* fp)(const sensor_msgs::msg::Image::ConstSharedPtr &), T * obj,
+    rmw_qos_profile_t custom_qos = rmw_qos_profile_default)
   {
-    return subscribe(base_topic, queue_size, std::bind(fp, obj.get(), std::placeholders::_1), obj); /*, transport_hints);*/
+    return subscribe(base_topic,
+             std::bind(fp, obj, std::placeholders::_1), custom_qos);
   }
 
   /*!
    * \brief Advertise a synchronized camera raw image + info topic pair, simple version.
    */
-  CameraPublisher advertiseCamera(const std::string& base_topic, uint32_t queue_size, bool latch = false);
+  CameraPublisher advertiseCamera(
+    const std::string & base_topic,
+    rmw_qos_profile_t custom_qos = rmw_qos_profile_default);
 
-  /*!
-   * \brief Advertise a synchronized camera raw image + info topic pair with subscriber status
-   * callbacks.
-   */
-  /*
-   * TODO(ros2) subscriber status callbacks
-  CameraPublisher advertiseCamera(const std::string& base_topic, uint32_t queue_size,
-                                  const SubscriberStatusCallback& image_connect_cb,
-                                  const SubscriberStatusCallback& image_disconnect_cb = SubscriberStatusCallback(),
-                                  const ros::SubscriberStatusCallback& info_connect_cb = ros::SubscriberStatusCallback(),
-                                  const ros::SubscriberStatusCallback& info_disconnect_cb = ros::SubscriberStatusCallback(),
-                                  const std::shared_ptr<void>& tracked_object = std::shared_ptr<void>(), bool latch = false);
-
-   */
   /**
    * \brief Subscribe to a synchronized image & camera info topic pair, version for arbitrary
    * std::function object.
    *
    * This version assumes the standard topic naming scheme, where the info topic is
    * named "camera_info" in the same namespace as the base image topic.
    */
-  CameraSubscriber subscribeCamera(const std::string& base_topic, uint32_t queue_size,
-                                   const CameraSubscriber::Callback& callback);
-  /*
-   * TODO(ros2) transport hints)
-                                   const std::shared_ptr<void>& tracked_object = std::shared_ptr<void>(),
-                                   const TransportHints& transport_hints = TransportHints());
-                                   */
+  CameraSubscriber subscribeCamera(
+    const std::string & base_topic,
+    const CameraSubscriber::Callback & callback,
+    rmw_qos_profile_t custom_qos = rmw_qos_profile_default);
 
   /**
    * \brief Subscribe to a synchronized image & camera info topic pair, version for bare function.
    */
-  CameraSubscriber subscribeCamera(const std::string& base_topic, uint32_t queue_size,
-                                   void(*fp)(const sensor_msgs::msg::Image::ConstSharedPtr&,
-                                             const sensor_msgs::msg::CameraInfo::ConstSharedPtr&))
-                                   /*
-                                    * TODO(ros2) transport hints
-                                   const TransportHints& transport_hints = TransportHints())
-                                   */
+  CameraSubscriber subscribeCamera(
+    const std::string & base_topic,
+    void (*fp)(const sensor_msgs::msg::Image::ConstSharedPtr &,
+    const sensor_msgs::msg::CameraInfo::ConstSharedPtr &),
+    rmw_qos_profile_t custom_qos = rmw_qos_profile_default)
   {
-    return subscribeCamera(base_topic, queue_size, CameraSubscriber::Callback(fp));/*,/ std::shared_ptr<void>(),
-                           transport_hints);
-                           */
+    return subscribeCamera(base_topic, CameraSubscriber::Callback(fp), custom_qos);
   }
 
   /**
    * \brief Subscribe to a synchronized image & camera info topic pair, version for class member
    * function with bare pointer.
    */
   template<class T>
-  CameraSubscriber subscribeCamera(const std::string& base_topic, uint32_t queue_size,
-                                   void(T::*fp)(const sensor_msgs::msg::Image::ConstSharedPtr&,
-                                                const sensor_msgs::msg::CameraInfo::ConstSharedPtr&), T* obj)
-                                   /*
-                                   const TransportHints& transport_hints = TransportHints())
-                                   */
-  {
-    return subscribeCamera(base_topic, queue_size, std::bind(fp, obj, std::placeholders::_1, std::placeholders::_2));
-    /*
-     * TODO(ros2) transport hints
-    , std::shared_ptr<void>(), transport_hints);
-    */
-  }
-
-  /**
-   * \brief Subscribe to a synchronized image & camera info topic pair, version for class member
-   * function with shared_ptr.
-   */
-  template<class T>
-  CameraSubscriber subscribeCamera(const std::string& base_topic, uint32_t queue_size,
-                                   void(T::*fp)(const sensor_msgs::msg::Image::ConstSharedPtr&,
-                                                const sensor_msgs::msg::CameraInfo::ConstSharedPtr&),
-                                   const std::shared_ptr<T>& obj)
-                                   /*
-                                    * TODO(ros2) transport hints
-                                   const TransportHints& transport_hints = TransportHints())
-                                   */
+  CameraSubscriber subscribeCamera(
+    const std::string & base_topic,
+    void (T::* fp)(const sensor_msgs::msg::Image::ConstSharedPtr &,
+    const sensor_msgs::msg::CameraInfo::ConstSharedPtr &), T * obj,
+    rmw_qos_profile_t custom_qos = rmw_qos_profile_default)
   {
-    return subscribeCamera(base_topic, queue_size, std::bind(fp, obj.get(), std::placeholders::_1, std::placeholders::_2), obj);
-                           /*transport_hints);*/
+    return subscribeCamera(base_topic,
+             std::bind(fp, obj, std::placeholders::_1, std::placeholders::_2), custom_qos);
   }
 
   /**

diff --git a/image_transport/include/image_transport/publisher_plugin.h b/image_transport/include/image_transport/publisher_plugin.h
@@ -35,6 +35,8 @@
 #ifndef IMAGE_TRANSPORT_PUBLISHER_PLUGIN_H
 #define IMAGE_TRANSPORT_PUBLISHER_PLUGIN_H
 
+#include <rclcpp/node.hpp>
+
 #include <sensor_msgs/msg/image.hpp>
 #include "image_transport/single_subscriber_publisher.h"
 
@@ -132,7 +134,7 @@ class PublisherPlugin
   /**
    * \brief Advertise a topic. Must be implemented by the subclass.
    */
-  virtual void advertiseImpl(rclcpp::Node::SharedPtr& nh, const std::string& base_topic, rmw_qos_profile_t custom_qos);
+  virtual void advertiseImpl(rclcpp::Node::SharedPtr& nh, const std::string& base_topic, rmw_qos_profile_t custom_qos) = 0;
 };
 
 } //namespace image_transport

diff --git a/image_transport/include/image_transport/raw_publisher.h b/image_transport/include/image_transport/raw_publisher.h
@@ -35,6 +35,7 @@
 #ifndef IMAGE_TRANSPORT_RAW_PUBLISHER_H
 #define IMAGE_TRANSPORT_RAW_PUBLISHER_H
 
+#include <sensor_msgs/msg/image.hpp>
 #include "image_transport/simple_publisher_plugin.h"
 
 namespace image_transport {
@@ -55,16 +56,6 @@ class RawPublisher : public SimplePublisherPlugin<sensor_msgs::msg::Image>
     return "raw";
   }
 
-  // Override the default implementation because publishing the message pointer allows
-  // the no-copy intraprocess optimization.
-  virtual void publish(const sensor_msgs::msg::Image::ConstSharedPtr& message) const
-  {
-    getPublisher().publish(message);
-  }
-
-  // Override the default implementation to not copy data to a sensor_msgs::msg::Image first
-  virtual void publish(const sensor_msgs::msg::Image& message, const uint8_t* data) const;
-
 protected:
   virtual void publish(const sensor_msgs::msg::Image& message, const PublishFn& publish_fn) const
   {

diff --git a/image_transport/include/image_transport/raw_subscriber.h b/image_transport/include/image_transport/raw_subscriber.h
@@ -35,6 +35,7 @@
 #ifndef IMAGE_TRANSPORT_RAW_SUBSCRIBER_H
 #define IMAGE_TRANSPORT_RAW_SUBSCRIBER_H
 
+#include <sensor_msgs/msg/image.hpp>
 #include "image_transport/simple_subscriber_plugin.h"
 
 namespace image_transport {
@@ -56,7 +57,7 @@ class RawSubscriber : public SimpleSubscriberPlugin<sensor_msgs::msg::Image>
   }
 
 protected:
-  virtual void internalCallback(const sensor_msgs::msg::Image::ConstSharedPtr& message, const Callback& user_cb)
+  virtual void internalCallback(const std::shared_ptr<const sensor_msgs::msg::Image> message, const Callback& user_cb)
   {
     user_cb(message);
   }