crosstyan/RapidPoseTriangulation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Create custom ros message instead of using json strings.

Browse Source
This commit is contained in:
Daniel
2025-02-05 12:11:22 +01:00
parent c1cc6054d1
commit 1fb730ff55
9 changed files with 103 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
extras/ros/dockerfile
View File

@ -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

2
extras/ros/pose2d_visualizer/package.xml
View File

@ -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>

13
extras/ros/pose2d_visualizer/pose2d_visualizer/pose2d_visualizer.py
View File

@ -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,

4
extras/ros/rpt2d_wrapper_cpp/CMakeLists.txt
View File

@ -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>)

2
extras/ros/rpt2d_wrapper_cpp/package.xml
View File

@ -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>

44
extras/ros/rpt2d_wrapper_cpp/src/rpt2d_wrapper.cpp
View File

@ -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>(

33
extras/ros/rpt_msgs/CMakeLists.txt Normal file
View File

@ -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()

7
extras/ros/rpt_msgs/msg/Pose.msg Normal file
View File

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

23
extras/ros/rpt_msgs/package.xml Normal file
View File

@ -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>