Some small updates.

extras/ros/pose2d_visualizer/pose2d_visualizer/pose2d_visualizer.py

@@ -10,7 +10,7 @@ from cv_bridge import CvBridge
 from rclpy.qos import QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy
 from sensor_msgs.msg import Image
 
-from rpt_msgs.msg import Pose
+from rpt_msgs.msg import Poses
 from skelda import utils_view
 
 # ==================================================================================================
@@ -130,7 +130,7 @@ def main():
         qos_profile,
     )
     _ = node.create_subscription(
-        Pose,
+        Poses,
         pose_input_topic,
         callback_poses,
         qos_profile,

extras/ros/rpt2d_wrapper_cpp/src/rpt2d_wrapper.cpp

@@ -18,7 +18,7 @@
 #include "/RapidPoseTriangulation/extras/include/nlohmann/json.hpp"
 using json = nlohmann::json;
 
-#include "rpt_msgs/msg/pose.hpp"
+#include "rpt_msgs/msg/poses.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<rpt_msgs::msg::Pose>(pose_out_topic, qos_profile);
+        pose_pub_ = this->create_publisher<rpt_msgs::msg::Poses>(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<rpt_msgs::msg::Pose>::SharedPtr pose_pub_;
+    rclcpp::Publisher<rpt_msgs::msg::Poses>::SharedPtr pose_pub_;
     std::atomic<bool> is_busy;
 
     // Pose model pointer
@@ -148,17 +148,16 @@ void Rpt2DWrapperNode::callback_images(const sensor_msgs::msg::Image::SharedPtr
         {"z-trigger-pose", z_trigger_pose}};
 
     // Publish message
-    auto pose_msg = rpt_msgs::msg::Pose();
+    auto pose_msg = rpt_msgs::msg::Poses();
     pose_msg.header = msg->header;
-    pose_msg.bodies_shape.push_back(valid_poses.size());
+    std::vector<int32_t> bshape = {(int)valid_poses.size(), (int)joint_names_2d.size(), 3};
-    pose_msg.bodies_shape.push_back(joint_names_2d.size());
+    pose_msg.bodies_shape = bshape;
-    pose_msg.bodies_shape.push_back(3);
+    pose_msg.bodies_flat.reserve(bshape[0] * bshape[1] * bshape[2]);
-    pose_msg.bodies_flat.reserve(valid_poses.size() * joint_names_2d.size() * 3);
+    for (size_t i = 0; i < bshape[0]; i++)
-    for (size_t i=0; i<valid_poses.size(); i++)
     {
-        for (size_t j=0; j<joint_names_2d.size(); j++)
+        for (size_t j = 0; j < bshape[1]; j++)
         {
-            for (size_t k=0; k<3; k++)
+            for (size_t k = 0; k < bshape[2]; k++)
             {
                 pose_msg.bodies_flat.push_back(valid_poses[i][j][k]);
             }

extras/ros/rpt_msgs/CMakeLists.txt

@@ -14,7 +14,7 @@ find_package(rosidl_default_generators REQUIRED)
 # find_package(<dependency> REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
-  "msg/Pose.msg"
+  "msg/Poses.msg"
   DEPENDENCIES std_msgs
 )