Create custom ros message instead of using json strings.

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;
-
-    // Build JSON
-    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"] = {
+    json jdata;
+    jdata["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(poses_msg);
+    // Publish message
+    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>(