Create custom ros message instead of using json strings.

extras/ros/dockerfile

@@ -45,12 +45,14 @@ RUN echo "source /project/dev_ws/install/setup.bash" >> ~/.bashrc
 # Copy modules
 COPY ./extras/include/ /RapidPoseTriangulation/extras/include/
 COPY ./scripts/ /RapidPoseTriangulation/scripts/
+COPY ./extras/ros/rpt_msgs/ /RapidPoseTriangulation/extras/ros/rpt_msgs/
 COPY ./extras/ros/pose2d_visualizer/ /RapidPoseTriangulation/extras/ros/pose2d_visualizer/
 COPY ./extras/ros/rpt2d_wrapper_cpp/ /RapidPoseTriangulation/extras/ros/rpt2d_wrapper_cpp/
 
 # Link and build as ros package
-RUN ln -s  /RapidPoseTriangulation/extras/ros/pose2d_visualizer/ /project/dev_ws/src/pose2d_visualizer
+RUN ln -s /RapidPoseTriangulation/extras/ros/rpt_msgs/ /project/dev_ws/src/
-RUN ln -s  /RapidPoseTriangulation/extras/ros/rpt2d_wrapper_cpp/ /project/dev_ws/src/rpt2d_wrapper_cpp
+RUN ln -s /RapidPoseTriangulation/extras/ros/pose2d_visualizer/ /project/dev_ws/src/
+RUN ln -s /RapidPoseTriangulation/extras/ros/rpt2d_wrapper_cpp/ /project/dev_ws/src/
 RUN /bin/bash -i -c 'cd /project/dev_ws/; colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release'
 
 # Update ros packages -> autocompletion and check

extras/ros/pose2d_visualizer/package.xml

@@ -7,6 +7,8 @@
   <maintainer email="root@todo.todo">root</maintainer>
   <license>TODO: License declaration</license>
 
+  <exec_depend>rpt_msgs</exec_depend>
+
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
   <test_depend>ament_pep257</test_depend>

extras/ros/pose2d_visualizer/pose2d_visualizer/pose2d_visualizer.py

@@ -1,4 +1,3 @@
-import json
 import sys
 import threading
 import time
@@ -10,8 +9,8 @@ import rclpy
 from cv_bridge import CvBridge
 from rclpy.qos import QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy
 from sensor_msgs.msg import Image
-from std_msgs.msg import String
 
+from rpt_msgs.msg import Pose
 from skelda import utils_view
 
 # ==================================================================================================
@@ -70,8 +69,9 @@ def callback_poses(pose_data):
     if last_input_image is None:
         return
 
-    # Convert pose data from json string
+    # Extract pose data
-    poses = json.loads(pose_data.data)
+    joint_names = pose_data.joint_names
+    bodies2D = np.array(pose_data.bodies_flat).reshape(pose_data.bodies_shape).tolist()
 
     # Collect inputs
     images_2d = []
@@ -83,12 +83,11 @@ def callback_poses(pose_data):
         timestamps.append(ts)
 
     # Visualize 2D poses
-    bodies2D = poses["bodies2D"]
     colors = plt.cm.hsv(np.linspace(0, 1, len(bodies2D), endpoint=False)).tolist()
     colors = [[int(c[0] * 255), int(c[1] * 255), int(c[2] * 255)] for c in colors]
     for i, body in enumerate(bodies2D):
         color = list(reversed(colors[i]))
-        img = utils_view.draw_body_in_image(img, body, poses["joints"], color)
+        img = utils_view.draw_body_in_image(img, body, joint_names, color)
 
     # Publish image with poses
     publish(img)
@@ -131,7 +130,7 @@ def main():
         qos_profile,
     )
     _ = node.create_subscription(
-        String,
+        Pose,
         pose_input_topic,
         callback_poses,
         qos_profile,

extras/ros/rpt2d_wrapper_cpp/CMakeLists.txt

@@ -18,7 +18,7 @@ endif()
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
-find_package(std_msgs REQUIRED)
+find_package(rpt_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(cv_bridge REQUIRED)
 find_package(OpenCV REQUIRED)
@@ -34,7 +34,7 @@ include_directories(/usr/local/include/onnxruntime/)
 link_directories(/usr/local/lib/)
 
 add_executable(rpt2d_wrapper src/rpt2d_wrapper.cpp)
-ament_target_dependencies(rpt2d_wrapper rclcpp std_msgs sensor_msgs cv_bridge)
+ament_target_dependencies(rpt2d_wrapper rclcpp sensor_msgs rpt_msgs cv_bridge)
 target_include_directories(rpt2d_wrapper PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)

extras/ros/rpt2d_wrapper_cpp/package.xml

@@ -10,7 +10,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>rclcpp</depend>
-  <depend>std_msgs</depend>
+  <depend>rpt_msgs</depend>
   <depend>sensor_msgs</depend>
 
   <depend>cv_bridge</depend>

extras/ros/rpt2d_wrapper_cpp/src/rpt2d_wrapper.cpp

@@ -9,7 +9,6 @@
 // ROS2
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/image.hpp>
-#include <std_msgs/msg/string.hpp>
 
 // OpenCV / cv_bridge
 #include <cv_bridge/cv_bridge.h>
@@ -19,6 +18,7 @@
 #include "/RapidPoseTriangulation/extras/include/nlohmann/json.hpp"
 using json = nlohmann::json;
 
+#include "rpt_msgs/msg/pose.hpp"
 #include "/RapidPoseTriangulation/scripts/utils_2d_pose.hpp"
 #include "/RapidPoseTriangulation/scripts/utils_pipeline.hpp"
 
@@ -60,7 +60,7 @@ public:
             std::bind(&Rpt2DWrapperNode::callback_images, this, std::placeholders::_1));
 
         // Setup publisher
-        pose_pub_ = this->create_publisher<std_msgs::msg::String>(pose_out_topic, qos_profile);
+        pose_pub_ = this->create_publisher<rpt_msgs::msg::Pose>(pose_out_topic, qos_profile);
 
         // Load model
         bool use_wb = utils_pipeline::use_whole_body(whole_body);
@@ -72,7 +72,7 @@ public:
 
 private:
     rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
-    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pose_pub_;
+    rclcpp::Publisher<rpt_msgs::msg::Pose>::SharedPtr pose_pub_;
     std::atomic<bool> is_busy;
 
     // Pose model pointer
@@ -82,13 +82,6 @@ private:
     void callback_images(const sensor_msgs::msg::Image::SharedPtr msg);
 
     std::vector<std::vector<std::array<float, 3>>> call_model(const cv::Mat &image);
-
-    void publish(const json &data)
-    {
-        std_msgs::msg::String msg;
-        msg.data = data.dump();
-        pose_pub_->publish(msg);
-    }
 };
 
 // =================================================================================================
@@ -145,13 +138,8 @@ void Rpt2DWrapperNode::callback_images(const sensor_msgs::msg::Image::SharedPtr
     double z_trigger_image = ts_image_sec - time_stamp;
     double z_trigger_pose = ts_pose_sec - time_stamp;
     double z_image_pose = ts_pose_sec - ts_image_sec;
-
+    json jdata;
-    // Build JSON
+    jdata["timestamps"] = {
-    json poses_msg;
-    poses_msg["bodies2D"] = valid_poses; // shape: persons x keypoints x 3
-    poses_msg["joints"] = joint_names_2d;
-    poses_msg["num_persons"] = valid_poses.size();
-    poses_msg["timestamps"] = {
         {"trigger", time_stamp},
         {"image", ts_image_sec},
         {"pose", ts_pose_sec},
@@ -159,8 +147,26 @@ void Rpt2DWrapperNode::callback_images(const sensor_msgs::msg::Image::SharedPtr
         {"z-image-pose", z_image_pose},
         {"z-trigger-pose", z_trigger_pose}};
 
-    // Publish
+    // Publish message
-    publish(poses_msg);
+    auto pose_msg = rpt_msgs::msg::Pose();
+    pose_msg.header = msg->header;
+    pose_msg.bodies_shape.push_back(valid_poses.size());
+    pose_msg.bodies_shape.push_back(joint_names_2d.size());
+    pose_msg.bodies_shape.push_back(3);
+    pose_msg.bodies_flat.reserve(valid_poses.size() * joint_names_2d.size() * 3);
+    for (size_t i=0; i<valid_poses.size(); i++)
+    {
+        for (size_t j=0; j<joint_names_2d.size(); j++)
+        {
+            for (size_t k=0; k<3; k++)
+            {
+                pose_msg.bodies_flat.push_back(valid_poses[i][j][k]);
+            }
+        }
+    }
+    pose_msg.joint_names = joint_names_2d;
+    pose_msg.extra_data = jdata.dump();
+    pose_pub_->publish(pose_msg);
 
     // Print info
     double elapsed_time = std::chrono::duration<double>(

extras/ros/rpt_msgs/CMakeLists.txt

@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.8)
+project(rpt_msgs)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/Pose.msg"
+  DEPENDENCIES std_msgs
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

extras/ros/rpt_msgs/msg/Pose.msg

@@ -0,0 +1,7 @@
+std_msgs/Header header
+
+float32[] bodies_flat
+int32[] bodies_shape
+string[] joint_names
+
+string extra_data

extras/ros/rpt_msgs/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>rpt_msgs</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="root@todo.todo">root</maintainer>
+  <license>TODO: License declaration</license>
+
+  <depend>std_msgs</depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>