[Ros2/Sink] Add draft of tensor_sink for ROS2

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,20 @@ 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)
+IF(WITH_ROS1)
+  SET(NNS_ROS_BRIDGE nns_ros_bridge)
+ELSEIF(WITH_ROS2)
+  SET(NNS_ROS_BRIDGE nns_ros_bridge2)
+ENDIF(WITH_ROS1)
+
+ADD_LIBRARY(tensor_ros_sink SHARED tensor_ros_sink.c)
+ADD_DEPENDENCIES(tensor_ros_sink ${NNS_ROS_BRIDGE})
 TARGET_INCLUDE_DIRECTORIES(tensor_ros_sink PRIVATE
   ${PKGS_INCLUDE_DIRS}
 )
 
 TARGET_LINK_LIBRARIES(tensor_ros_sink
-  nns_ros_bridge
+  ${NNS_ROS_BRIDGE}
   ${PKGS_COMMON_LIBRARIES}
   ${PKGS_LIBRARIES}
 )

gst/tensor_ros_sink/tensor_ros_sink.c

@@ -479,7 +479,8 @@ gst_tensor_ros_sink_finalize (GObject *object)
 
   g_mutex_clear (&self->mutex);
   g_free (self->location);
-  nns_ros_publisher_close_bag (self->rosbag_to_save);
+  nns_ros_publisher_close_bag (self->nns_ros_bind_instance,
+      self->rosbag_to_save);
   nns_ros_publisher_finalize (self->nns_ros_bind_instance);
 
   self->location = NULL;

include/nns_ros_publisher.h

@@ -23,6 +23,8 @@
 typedef enum _err_code {
   ROS1_UNDEFINED_ROS_MASTER_URI,
   ROS1_FAILED_TO_CONNECT_ROSCORE,
+  ROS2_FAILED_TO_CREATE_NODE,
+  ROS2_FAILED_TO_CREATE_PUBLISHER,
 } err_code;
 
 #ifdef __cplusplus
@@ -130,7 +132,7 @@ gboolean nns_ros_publisher_set_pub_topic (void *instance,
     const GstTensorsConfig *conf);
 const gchar *nns_ros_publisher_get_pub_topic_name (void *instance);
 void *nns_ros_publisher_open_writable_bag (void *instance, const char *name);
-void nns_ros_publisher_close_bag (void *bag);
+void nns_ros_publisher_close_bag (void *instance, void *bag);
 #ifdef __cplusplus
 }
 #endif /* __cplusplus */

ros/node/nns_roscpp_publisher.cc

@@ -225,7 +225,8 @@ void *nns_ros_publisher_open_writable_bag (void *instance, const char *name)
   return (void *) bag;
 }
 
-void nns_ros_publisher_close_bag(void *bag)
+void nns_ros_publisher_close_bag(void * instance __attribute__((unused)),
+    void *bag)
 {
   rosbag::Bag *rosbag = (rosbag::Bag *) bag;
 

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,193 @@
+/* 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 "rclcpp/rclcpp.hpp"
+#include "nns_ros_publisher.h"
+#include "nns_ros2_bridge/msg/tensors.hpp"
+#include "std_msgs/msg/multi_array_layout.hpp"
+#include "std_msgs/msg/multi_array_dimension.hpp"
+
+using namespace std_msgs::msg;
+
+class NnsRclCppPublisher
+    : public NnsRosPublisher<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";
+
+template <>
+std::string NnsRosPublisher<MultiArrayLayout, size_t>::str_nns_helper_name =
+    "nns_rclcpp_publisher";
+
+NnsRclCppPublisher::NnsRclCppPublisher (const char *node_name,
+    const char *topic_name, gboolean is_dummy)
+    : NnsRosPublisher<MultiArrayLayout, size_t> (node_name, topic_name,
+        is_dummy)
+{
+  /* RCLCPP initialization requires command-line arguments */
+  int dummy_argc = 0;
+  char **dummy_argv = NULL;
+
+  try {
+    rclcpp::init (dummy_argc, dummy_argv);
+  } catch (rclcpp::ContextAlreadyInitialized &e) {
+    g_warning ("%s: The given context has been already initialized",
+        (this->str_nns_helper_name).c_str ());
+  }
+
+  try {
+    this->node_ = rclcpp::Node::make_shared (node_name,
+      NnsRclCppPublisher::node_namespace_);
+  } catch (rclcpp::exceptions::RCLErrorBase &e) {
+    g_critical ("%s: Failed to create a rclcpp::Node instance: %s",
+        (this->str_nns_helper_name).c_str (), e.message.c_str ());
+    throw ROS2_FAILED_TO_CREATE_NODE;
+  }
+
+  try {
+    this->publisher_ =
+      this->node_->create_publisher<nns_ros2_bridge::msg::Tensors> (
+          topic_name, default_q_size);
+  } catch (rclcpp::exceptions::RCLErrorBase &e) {
+    g_critical ("%s: Failed to create a rclcpp::Node instance: %s",
+        (this->str_nns_helper_name).c_str (), e.message.c_str ());
+    throw ROS2_FAILED_TO_CREATE_PUBLISHER;
+  }
+}
+
+NnsRclCppPublisher::~NnsRclCppPublisher ()
+{
+  bool ret = rclcpp::shutdown();
+
+  if (!ret)
+    g_critical ("%s: Failed to shutdown the given context",
+        (this->str_nns_helper_name).c_str ());
+}
+
+/**
+ * @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) {
+    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)
+{
+  try {
+    return new NnsRclCppPublisher (node_name, topic_name, is_dummy);
+  } catch (const err_code e) {
+    return nullptr;
+  }
+}
+
+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<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 * instance __attribute__((unused)), 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>