From 1fb730ff55638d08e244d1c9b3fe06cf9d42455b Mon Sep 17 00:00:00 2001 From: Daniel Date: Wed, 5 Feb 2025 12:11:22 +0100 Subject: [PATCH] Create custom ros message instead of using json strings. --- extras/ros/dockerfile | 6 ++- extras/ros/pose2d_visualizer/package.xml | 2 + .../pose2d_visualizer/pose2d_visualizer.py | 13 +++--- extras/ros/rpt2d_wrapper_cpp/CMakeLists.txt | 4 +- extras/ros/rpt2d_wrapper_cpp/package.xml | 2 +- .../rpt2d_wrapper_cpp/src/rpt2d_wrapper.cpp | 44 +++++++++++-------- extras/ros/rpt_msgs/CMakeLists.txt | 33 ++++++++++++++ extras/ros/rpt_msgs/msg/Pose.msg | 7 +++ extras/ros/rpt_msgs/package.xml | 23 ++++++++++ 9 files changed, 103 insertions(+), 31 deletions(-) create mode 100644 extras/ros/rpt_msgs/CMakeLists.txt create mode 100644 extras/ros/rpt_msgs/msg/Pose.msg create mode 100644 extras/ros/rpt_msgs/package.xml diff --git a/extras/ros/dockerfile b/extras/ros/dockerfile index 26d9da0..fe72c48 100644 --- a/extras/ros/dockerfile +++ b/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/rpt2d_wrapper_cpp/ /project/dev_ws/src/rpt2d_wrapper_cpp +RUN ln -s /RapidPoseTriangulation/extras/ros/rpt_msgs/ /project/dev_ws/src/ +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 diff --git a/extras/ros/pose2d_visualizer/package.xml b/extras/ros/pose2d_visualizer/package.xml index 5334080..99c3f33 100644 --- a/extras/ros/pose2d_visualizer/package.xml +++ b/extras/ros/pose2d_visualizer/package.xml @@ -7,6 +7,8 @@ root TODO: License declaration + rpt_msgs + ament_copyright ament_flake8 ament_pep257 diff --git a/extras/ros/pose2d_visualizer/pose2d_visualizer/pose2d_visualizer.py b/extras/ros/pose2d_visualizer/pose2d_visualizer/pose2d_visualizer.py index 39135c3..869e5b2 100644 --- a/extras/ros/pose2d_visualizer/pose2d_visualizer/pose2d_visualizer.py +++ b/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 - poses = json.loads(pose_data.data) + # Extract pose 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, diff --git a/extras/ros/rpt2d_wrapper_cpp/CMakeLists.txt b/extras/ros/rpt2d_wrapper_cpp/CMakeLists.txt index 7e89536..6b4f8c3 100644 --- a/extras/ros/rpt2d_wrapper_cpp/CMakeLists.txt +++ b/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 $ $) diff --git a/extras/ros/rpt2d_wrapper_cpp/package.xml b/extras/ros/rpt2d_wrapper_cpp/package.xml index 53e24cc..7a40658 100644 --- a/extras/ros/rpt2d_wrapper_cpp/package.xml +++ b/extras/ros/rpt2d_wrapper_cpp/package.xml @@ -10,7 +10,7 @@ ament_cmake rclcpp - std_msgs + rpt_msgs sensor_msgs cv_bridge diff --git a/extras/ros/rpt2d_wrapper_cpp/src/rpt2d_wrapper.cpp b/extras/ros/rpt2d_wrapper_cpp/src/rpt2d_wrapper.cpp index a5ecfce..1d92bf4 100644 --- a/extras/ros/rpt2d_wrapper_cpp/src/rpt2d_wrapper.cpp +++ b/extras/ros/rpt2d_wrapper_cpp/src/rpt2d_wrapper.cpp @@ -9,7 +9,6 @@ // ROS2 #include #include -#include // OpenCV / cv_bridge #include @@ -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(pose_out_topic, qos_profile); + pose_pub_ = this->create_publisher(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::SharedPtr image_sub_; - rclcpp::Publisher::SharedPtr pose_pub_; + rclcpp::Publisher::SharedPtr pose_pub_; std::atomic is_busy; // Pose model pointer @@ -82,13 +82,6 @@ private: void callback_images(const sensor_msgs::msg::Image::SharedPtr msg); std::vector>> 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; ipublish(pose_msg); // Print info double elapsed_time = std::chrono::duration( diff --git a/extras/ros/rpt_msgs/CMakeLists.txt b/extras/ros/rpt_msgs/CMakeLists.txt new file mode 100644 index 0000000..cb29064 --- /dev/null +++ b/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( 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() diff --git a/extras/ros/rpt_msgs/msg/Pose.msg b/extras/ros/rpt_msgs/msg/Pose.msg new file mode 100644 index 0000000..96ad74b --- /dev/null +++ b/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 diff --git a/extras/ros/rpt_msgs/package.xml b/extras/ros/rpt_msgs/package.xml new file mode 100644 index 0000000..7c1fda5 --- /dev/null +++ b/extras/ros/rpt_msgs/package.xml @@ -0,0 +1,23 @@ + + + + rpt_msgs + 0.0.0 + TODO: Package description + root + TODO: License declaration + + std_msgs + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + +