[Ros2/Sink] Add draft of tensor_sink for ROS2

This patch adds a draft of the NNStreamer extension plugin,
tensor_ros2_sink, that publishes streams of tensors as a ROS2 topic.

Signed-off-by: Wook Song <[email protected]>

gst/CMakeLists.txt

@@ -1,4 +1,4 @@
+ADD_SUBDIRECTORY(tensor_ros_sink)
 IF(WITH_ROS1)
-  ADD_SUBDIRECTORY(tensor_ros_sink)
   ADD_SUBDIRECTORY(tensor_ros_src)
 ENDIF(WITH_ROS1)

gst/tensor_ros_sink/CMakeLists.txt

@@ -4,14 +4,14 @@ SET(PKG_MODULES
 
 PKG_CHECK_MODULES(PKGS REQUIRED ${PKG_MODULES})
 
-ADD_LIBRARY(tensor_ros_sink tensor_ros_sink.c)
-ADD_DEPENDENCIES(tensor_ros_sink nns_ros_bridge)
+ADD_LIBRARY(tensor_ros_sink SHARED tensor_ros_sink.c)
+ADD_DEPENDENCIES(tensor_ros_sink nns_ros_bridge2)
 TARGET_INCLUDE_DIRECTORIES(tensor_ros_sink PRIVATE
   ${PKGS_INCLUDE_DIRS}
 )
 
 TARGET_LINK_LIBRARIES(tensor_ros_sink
-  nns_ros_bridge
+  nns_ros_bridge2
   ${PKGS_COMMON_LIBRARIES}
   ${PKGS_LIBRARIES}
 )

ros2/CMakeLists.txt

@@ -40,19 +40,24 @@ FIND_PACKAGE(std_msgs REQUIRED)
 
 FIND_PACKAGE(rosidl_default_generators REQUIRED)
 
-ROSIDL_GENERATE_INTERFACES(
-  ${PROJECT_NAME}
-  "${NNS_ROS_COMMON_DIR}/msg:Tensors.msg"
+SET(NNS_ROS2_LIB_MSGS "${NNS_ROS_COMMON_DIR}/msg:Tensors.msg")
+SET(NNS_ROS2_LIB_SRCS
+    node/nns_rclcpp_publisher.cc
 )
+SET(NNS_ROS2_LIB_TARGET nns_ros_bridge2)
 
-ADD_EXECUTABLE(publisher_member_function node/member_function.cpp)
-AMENT_TARGET_DEPENDENCIES(publisher_member_function rclcpp std_msgs)
+ROSIDL_GENERATE_INTERFACES(${PROJECT_NAME}
+    ${NNS_ROS2_LIB_MSGS}
+    DEPENDENCIES builtin_interfaces std_msgs
+)
 
-INSTALL(TARGETS
-  publisher_member_function
-  RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_BINDIR}
-  LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}
-  ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}
+ADD_LIBRARY(${NNS_ROS2_LIB_TARGET} SHARED
+    ${NNS_ROS2_LIB_SRCS}
+)
+AMENT_TARGET_DEPENDENCIES(${NNS_ROS2_LIB_TARGET} rclcpp std_msgs)
+AMENT_EXPORT_DEPENDENCIES(rosidl_default_runtime)
+ROSIDL_TARGET_INTERFACES(${NNS_ROS2_LIB_TARGET}
+    ${PROJECT_NAME} "rosidl_typesupport_cpp"
 )
 
 IF(BUILD_TESTING)
@@ -67,3 +72,8 @@ IF(BUILD_TESTING)
 ENDIF()
 
 AMENT_PACKAGE()
+INSTALL(TARGETS ${NNS_ROS2_LIB_TARGET}
+    RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_BINDIR}
+    LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}
+    ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}
+)

ros2/node/nns_rclcpp_publisher.cc

@@ -0,0 +1,162 @@
+/* SPDX-License-Identifier: LGPL-2.1-only */
+/**
+ * NNStreamer-ROS: NNStreamer extension packages for ROS/ROS2 support
+ * Copyright (C) 2020 Wook Song <[email protected]>
+ * Copyright (C) 2018 Samsung Electronics Co., Ltd.
+ *
+ * @file        nns_rclcpp_publisher.cc
+ * @date        10/20/2020
+ * @brief       A helper class for publishing ROS2 (i.e., rclcpp) topic
+ *              via NNStreamer plugins
+ * @see         https://github.com/nnstreamer/nnstreamer-ros
+ * @author      Wook Song <[email protected]>
+ * @bug         No known bugs except for NYI items
+ *
+ * This class bridges between the NNStreamer (C) and ROS2 frameworks (ROSCPP/C++)
+ * by implementing the NnsRosPublisher class.
+ *
+ */
+
+#include <chrono>
+#include <memory>
+
+#include <std_msgs/msg/multi_array_layout.hpp>
+#include <std_msgs/msg/multi_array_dimension.hpp>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nns_ros_publisher.h"
+#include "nns_ros2_bridge/msg/tensors.hpp"
+
+
+class NnsRclCppPublisher
+    : public NnsRosPublisher<std_msgs::msg::MultiArrayLayout, size_t>
+{
+public:
+  NnsRclCppPublisher (const char *node_name, const char *topic_name,
+      gboolean is_dummy);
+  ~NnsRclCppPublisher ();
+
+  gboolean publish (const guint num_tensors,
+      const GstTensorMemory *tensors_mem, void *bag) final;
+
+private:
+  static const std::string node_namespace_;
+  rclcpp::Node::SharedPtr node_;
+  rclcpp::Publisher<nns_ros2_bridge::msg::Tensors>::SharedPtr publisher_;
+};
+
+const std::string NnsRclCppPublisher::node_namespace_ = "tensor_ros2_sink";
+
+NnsRclCppPublisher::NnsRclCppPublisher (const char *node_name,
+    const char *topic_name, gboolean is_dummy)
+    : NnsRosPublisher<std_msgs::msg::MultiArrayLayout, size_t> (node_name,
+        topic_name, is_dummy)
+{
+  /* RCLCPP initialization requires command-line arguments */
+  int dummy_argc = 0;
+  char **dummy_argv = NULL;
+
+  rclcpp::init (dummy_argc, dummy_argv);
+
+  this->node_ = rclcpp::Node::make_shared (node_name,
+      NnsRclCppPublisher::node_namespace_);
+  this->publisher_ =
+  this->node_->create_publisher<nns_ros2_bridge::msg::Tensors> (
+      topic_name, default_q_size);
+}
+
+NnsRclCppPublisher::~NnsRclCppPublisher ()
+{
+}
+
+/**
+ * @brief	A method for publishing a ROS topic that contains the tensors passed by the NNStreamer framework
+ * @param[in] num_tensors : The number of tensors included in tensors_mem (for the verification purpose)
+ * @param[in] tensors_mem : The pointer of containers consists of information and raw data of tensors
+ * @return TRUE if the configuration is successfully done
+ */
+gboolean NnsRclCppPublisher::publish (const guint num_tensors,
+    const GstTensorMemory *tensors_mem, void *bag __attribute__((unused)))
+{
+  nns_ros2_bridge::msg::Tensors tensors_msg;
+
+  g_return_val_if_fail (this->ready_to_pub, FALSE);
+  g_return_val_if_fail (num_tensors == this->num_of_tensors_pub, FALSE);
+
+  for (guint i = 0; i < this->num_of_tensors_pub; ++i) {
+    std_msgs::msg::UInt8MultiArray each_tensor;
+    uint8_t *src_data = (uint8_t *) tensors_mem[i].data;
+
+    each_tensor.layout = this->topic_layouts_pub[i];
+    each_tensor.data.assign (src_data, src_data + tensors_mem[i].size);
+
+    tensors_msg.tensors.push_back (each_tensor);
+  }
+
+  if (!this->is_dummy) {
+    g_return_val_if_fail (rclcpp::ok(), FALSE);
+    this->publisher_->publish (tensors_msg);
+    rclcpp::spin_some(this->node_);
+  }
+
+  return TRUE;
+}
+
+/**
+ * C functions for NNStreamer plugins that want to publish or subscribe
+ * ROS topics. nns_ros_publisher_init() should be invoked before configuring the
+ * tensors information to publish or subscribe. Each NNStreamer plugin which
+ * uses this class should be holding the instance of NnsRclCppPublisher (i.e., the
+ * void type pointer what the nns_ros_publisher_init() function returns).
+ */
+void *
+nns_ros_publisher_init (const char *node_name, const char *topic_name,
+    gboolean is_dummy)
+{
+  return new NnsRclCppPublisher (node_name, topic_name, is_dummy);
+}
+
+void
+nns_ros_publisher_finalize (void *instance)
+{
+  NnsRclCppPublisher *nrp_instance = (NnsRclCppPublisher *) instance;
+  if (nrp_instance != nullptr)
+    delete nrp_instance;
+}
+
+gboolean
+nns_ros_publisher_publish (void *instance, const guint num_tensors,
+    const GstTensorMemory *tensors_mem, void *bag)
+{
+  NnsRclCppPublisher *nrp_instance = (NnsRclCppPublisher *) instance;
+
+  return nrp_instance->publish (num_tensors, tensors_mem, bag);
+}
+
+gboolean
+nns_ros_publisher_set_pub_topic (void *instance, const GstTensorsConfig *conf)
+{
+  NnsRclCppPublisher *nrp_instance = (NnsRclCppPublisher *) instance;
+
+  return nrp_instance->setPubTopicInfo<std_msgs::msg::MultiArrayDimension> (conf);
+}
+
+const gchar *
+nns_ros_publisher_get_pub_topic_name (void *instance)
+{
+  NnsRclCppPublisher *nrp_instance = (NnsRclCppPublisher *) instance;
+
+  return nrp_instance->getPubTopicName();
+}
+
+void *
+nns_ros_publisher_open_writable_bag (void * instance __attribute__((unused)), const char *name __attribute__((unused)))
+{
+  return nullptr;
+}
+
+void
+nns_ros_publisher_close_bag (void *bag __attribute__((unused)))
+{
+  ;
+}

ros2/package.xml

@@ -11,7 +11,6 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>rclcpp</build_depend>
   <build_depend>std_msgs</build_depend>
-
   <build_depend>rosidl_default_generators</build_depend>
 
   <exec_depend>rclcpp</exec_depend>