From 0a2f04a1f87d7167d2e405da8bac6f241b33e9a8 Mon Sep 17 00:00:00 2001 From: Daniel Date: Mon, 1 Sep 2025 18:25:52 +0200 Subject: [PATCH] Also integrated visual ros markers. --- extras/ros/docker-compose-3d.yml | 28 +- extras/ros/dockerfile_3d | 4 +- .../CMakeLists.txt | 22 +- .../package.xml | 3 +- .../marker_publishers/src/cube_markers.cpp | 122 +++++++++ .../src/skeleton_markers.cpp | 255 ++++++++++++++++++ .../src/skeleton_tfs.cpp | 0 7 files changed, 428 insertions(+), 6 deletions(-) rename extras/ros/{transform_publisher => marker_publishers}/CMakeLists.txt (63%) rename extras/ros/{transform_publisher => marker_publishers}/package.xml (90%) create mode 100644 extras/ros/marker_publishers/src/cube_markers.cpp create mode 100644 extras/ros/marker_publishers/src/skeleton_markers.cpp rename extras/ros/{transform_publisher => marker_publishers}/src/skeleton_tfs.cpp (100%) diff --git a/extras/ros/docker-compose-3d.yml b/extras/ros/docker-compose-3d.yml index 1081886..b6266bf 100644 --- a/extras/ros/docker-compose-3d.yml +++ b/extras/ros/docker-compose-3d.yml @@ -32,6 +32,32 @@ services: - "PYTHONUNBUFFERED=1" command: /bin/bash -i -c 'export ROS_DOMAIN_ID=18 && ros2 run rpt3d_wrapper_cpp rpt3d_wrapper' + skeleton_markers: + image: rapidposetriangulation_ros3d + network_mode: "host" + ipc: "host" + privileged: true + volumes: + - ../../:/RapidPoseTriangulation/ + - /tmp/.X11-unix:/tmp/.X11-unix + - /dev/shm:/dev/shm + environment: + - "PYTHONUNBUFFERED=1" + command: /bin/bash -i -c 'export ROS_DOMAIN_ID=18 && ros2 run marker_publishers skeleton_markers' + + cube_markers: + image: rapidposetriangulation_ros3d + network_mode: "host" + ipc: "host" + privileged: true + volumes: + - ../../:/RapidPoseTriangulation/ + - /tmp/.X11-unix:/tmp/.X11-unix + - /dev/shm:/dev/shm + environment: + - "PYTHONUNBUFFERED=1" + command: /bin/bash -i -c 'export ROS_DOMAIN_ID=18 && ros2 run marker_publishers cube_markers' + skeleton_tfs: image: rapidposetriangulation_ros3d network_mode: "host" @@ -43,4 +69,4 @@ services: - /dev/shm:/dev/shm environment: - "PYTHONUNBUFFERED=1" - command: /bin/bash -i -c 'export ROS_DOMAIN_ID=18 && ros2 run transform_publisher skeleton_tfs' + command: /bin/bash -i -c 'export ROS_DOMAIN_ID=18 && ros2 run marker_publishers skeleton_tfs' diff --git a/extras/ros/dockerfile_3d b/extras/ros/dockerfile_3d index 9e4aa73..1196e69 100644 --- a/extras/ros/dockerfile_3d +++ b/extras/ros/dockerfile_3d @@ -24,14 +24,14 @@ RUN echo "source /project/dev_ws/install/setup.bash" >> ~/.bashrc # Copy modules COPY ./extras/include/ /RapidPoseTriangulation/extras/include/ COPY ./rpt/ /RapidPoseTriangulation/rpt/ +COPY ./extras/ros/marker_publishers/ /RapidPoseTriangulation/extras/ros/marker_publishers/ COPY ./extras/ros/rpt_msgs/ /RapidPoseTriangulation/extras/ros/rpt_msgs/ COPY ./extras/ros/rpt3d_wrapper_cpp/ /RapidPoseTriangulation/extras/ros/rpt3d_wrapper_cpp/ -COPY ./extras/ros/transform_publisher/ /RapidPoseTriangulation/extras/ros/transform_publisher/ # Link and build as ros package +RUN ln -s /RapidPoseTriangulation/extras/ros/marker_publishers/ /project/dev_ws/src/ RUN ln -s /RapidPoseTriangulation/extras/ros/rpt_msgs/ /project/dev_ws/src/ RUN ln -s /RapidPoseTriangulation/extras/ros/rpt3d_wrapper_cpp/ /project/dev_ws/src/ -RUN ln -s /RapidPoseTriangulation/extras/ros/transform_publisher/ /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/transform_publisher/CMakeLists.txt b/extras/ros/marker_publishers/CMakeLists.txt similarity index 63% rename from extras/ros/transform_publisher/CMakeLists.txt rename to extras/ros/marker_publishers/CMakeLists.txt index 38ecbb4..bd51c83 100644 --- a/extras/ros/transform_publisher/CMakeLists.txt +++ b/extras/ros/marker_publishers/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(transform_publisher) +project(marker_publishers) # Default to C99 if(NOT CMAKE_C_STANDARD) @@ -21,6 +21,19 @@ find_package(rclcpp REQUIRED) find_package(rpt_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) +find_package(visualization_msgs REQUIRED) + +add_executable(cube_markers src/cube_markers.cpp) +ament_target_dependencies(cube_markers rclcpp geometry_msgs visualization_msgs) +target_include_directories(cube_markers PUBLIC + $ + $) + +add_executable(skeleton_markers src/skeleton_markers.cpp) +ament_target_dependencies(skeleton_markers rclcpp rpt_msgs geometry_msgs visualization_msgs) +target_include_directories(skeleton_markers PUBLIC + $ + $) add_executable(skeleton_tfs src/skeleton_tfs.cpp) ament_target_dependencies(skeleton_tfs rclcpp rpt_msgs geometry_msgs tf2_ros) @@ -28,9 +41,14 @@ target_include_directories(skeleton_tfs PUBLIC $ $) -install(TARGETS skeleton_tfs +install(TARGETS cube_markers DESTINATION lib/${PROJECT_NAME}) +install(TARGETS skeleton_markers + DESTINATION lib/${PROJECT_NAME}) + +install(TARGETS skeleton_tfs + DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights diff --git a/extras/ros/transform_publisher/package.xml b/extras/ros/marker_publishers/package.xml similarity index 90% rename from extras/ros/transform_publisher/package.xml rename to extras/ros/marker_publishers/package.xml index eda98f4..2ca1087 100644 --- a/extras/ros/transform_publisher/package.xml +++ b/extras/ros/marker_publishers/package.xml @@ -1,7 +1,7 @@ - transform_publisher + marker_publishers 0.0.0 TODO: Package description root @@ -13,6 +13,7 @@ rpt_msgs geometry_msgs tf2_ros + visualization_msgs ament_lint_auto ament_lint_common diff --git a/extras/ros/marker_publishers/src/cube_markers.cpp b/extras/ros/marker_publishers/src/cube_markers.cpp new file mode 100644 index 0000000..403f2fd --- /dev/null +++ b/extras/ros/marker_publishers/src/cube_markers.cpp @@ -0,0 +1,122 @@ +#include +#include + +#include +#include +#include +#include + +// ================================================================================================= + +const std::string output_topic = "/markers_cube"; +static const std::array, 2> roomparams = {{ + {4.0, 5.0, 2.2}, + {1.0, 0.0, 1.1}, +}}; + +// ================================================================================================= +// ================================================================================================= + +class CubePublisher : public rclcpp::Node +{ +public: + CubePublisher() : Node("cube_publisher") + { + publisher_ = this->create_publisher(output_topic, 1); + timer_ = this->create_wall_timer( + std::chrono::seconds(1), std::bind(&CubePublisher::timer_callback, this)); + + cube_edges = generate_cube_edges(); + } + +private: + visualization_msgs::msg::MarkerArray cube_edges; + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::TimerBase::SharedPtr timer_; + + void timer_callback() + { + publisher_->publish(cube_edges); + } + visualization_msgs::msg::MarkerArray generate_cube_edges(); +}; + +// ================================================================================================= + +visualization_msgs::msg::MarkerArray CubePublisher::generate_cube_edges() +{ + visualization_msgs::msg::MarkerArray marker_array; + visualization_msgs::msg::Marker marker; + + marker.header.frame_id = "world"; + marker.type = visualization_msgs::msg::Marker::LINE_LIST; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.scale.x = 0.05; + marker.id = 0; + + marker.color.a = 1.0; + marker.color.r = 1.0; + + std::vector> corners = { + {-1, -1, -1}, + {1, -1, -1}, + {1, 1, -1}, + {-1, 1, -1}, + {-1, -1, 1}, + {1, -1, 1}, + {1, 1, 1}, + {-1, 1, 1}, + }; + + for (auto &corner : corners) + { + for (size_t i = 0; i < corner.size(); ++i) + { + corner[i] = 0.5 * roomparams[0][i] * corner[i] + roomparams[1][i]; + } + } + + std::vector> edge_indices = { + {0, 1}, + {1, 2}, + {2, 3}, + {3, 0}, + {4, 5}, + {5, 6}, + {6, 7}, + {7, 4}, + {0, 4}, + {1, 5}, + {2, 6}, + {3, 7}, + }; + + for (const auto &edge : edge_indices) + { + geometry_msgs::msg::Point p1, p2; + p1.x = corners[edge.first][0]; + p1.y = corners[edge.first][1]; + p1.z = corners[edge.first][2]; + + p2.x = corners[edge.second][0]; + p2.y = corners[edge.second][1]; + p2.z = corners[edge.second][2]; + + marker.points.push_back(p1); + marker.points.push_back(p2); + } + + marker_array.markers.push_back(marker); + return marker_array; +} + +// ================================================================================================= +// ================================================================================================= + +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/extras/ros/marker_publishers/src/skeleton_markers.cpp b/extras/ros/marker_publishers/src/skeleton_markers.cpp new file mode 100644 index 0000000..c4c3448 --- /dev/null +++ b/extras/ros/marker_publishers/src/skeleton_markers.cpp @@ -0,0 +1,255 @@ +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "rpt_msgs/msg/poses.hpp" + +// ================================================================================================= + +const std::string input_topic = "/poses/humans3d"; +const std::string output_topic = "/markers_skeleton"; + +// ================================================================================================= +// ================================================================================================= + +std::array hue_to_rgb(double hue) +{ + double r, g, b; + int h = static_cast(hue * 6); + double f = hue * 6 - h; + double q = 1 - f; + + switch (h % 6) + { + case 0: + r = 1; + g = f; + b = 0; + break; + case 1: + r = q; + g = 1; + b = 0; + break; + case 2: + r = 0; + g = 1; + b = f; + break; + case 3: + r = 0; + g = q; + b = 1; + break; + case 4: + r = f; + g = 0; + b = 1; + break; + case 5: + r = 1; + g = 0; + b = q; + break; + default: + r = g = b = 0; + break; + } + + std::array rgb = { + static_cast(r * 255), + static_cast(g * 255), + static_cast(b * 255)}; + + return rgb; +} + +// ================================================================================================= +// ================================================================================================= + +class SkeletonPublisher : public rclcpp::Node +{ +public: + SkeletonPublisher() : Node("skeleton_publisher") + { + publisher_ = this->create_publisher(output_topic, 1); + subscriber_ = this->create_subscription( + input_topic, 1, + std::bind(&SkeletonPublisher::listener_callback, this, std::placeholders::_1)); + + connections = { + {"shoulder_middle", "head"}, + {"head", "nose"}, + {"nose", "eye_left"}, + {"nose", "eye_right"}, + {"eye_left", "ear_left"}, + {"eye_right", "ear_right"}, + {"shoulder_left", "shoulder_right"}, + {"hip_middle", "shoulder_left"}, + {"hip_middle", "shoulder_right"}, + {"shoulder_left", "elbow_left"}, + {"elbow_left", "wrist_left"}, + {"hip_middle", "hip_left"}, + {"hip_left", "knee_left"}, + {"knee_left", "ankle_left"}, + {"shoulder_right", "elbow_right"}, + {"elbow_right", "wrist_right"}, + {"hip_middle", "hip_right"}, + {"hip_right", "knee_right"}, + {"knee_right", "ankle_right"}, + }; + } + +private: + std::vector> connections; + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Subscription::SharedPtr subscriber_; + + void listener_callback(const rpt_msgs::msg::Poses::SharedPtr msg); + visualization_msgs::msg::MarkerArray generate_msg( + const std::vector>> &poses, + const std::vector &joint_names); +}; + +// ================================================================================================= + +void SkeletonPublisher::listener_callback(const rpt_msgs::msg::Poses::SharedPtr msg) +{ + std::vector>> poses; + const std::vector &joint_names = msg->joint_names; + + // Unflatten poses + size_t idx = 0; + std::vector &bshape = msg->bodies_shape; + for (int32_t i = 0; i < bshape[0]; ++i) + { + std::vector> body; + for (int32_t j = 0; j < bshape[1]; ++j) + { + std::array joint; + for (int32_t k = 0; k < bshape[2]; ++k) + { + joint[k] = msg->bodies_flat[idx]; + ++idx; + } + body.push_back(std::move(joint)); + } + poses.push_back(std::move(body)); + } + + auto skelmsg = generate_msg(poses, joint_names); + publisher_->publish(skelmsg); +} + +// ================================================================================================= + +visualization_msgs::msg::MarkerArray SkeletonPublisher::generate_msg( + const std::vector>> &poses, + const std::vector &joint_names) +{ + visualization_msgs::msg::MarkerArray marker_array; + visualization_msgs::msg::Marker marker; + + marker.header.frame_id = "world"; + marker.type = visualization_msgs::msg::Marker::LINE_LIST; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.scale.x = 0.02; + marker.id = 0; + + size_t num_bodies = poses.size(); + for (size_t i = 0; i < num_bodies; ++i) + { + std_msgs::msg::ColorRGBA color; + double hue = static_cast(i) / num_bodies; + auto rgb = hue_to_rgb(hue); + color.r = rgb[0] / 255.0; + color.g = rgb[1] / 255.0; + color.b = rgb[2] / 255.0; + color.a = 1.0; + + for (size_t j = 0; j < connections.size(); ++j) + { + std::string joint1 = connections[j][0]; + std::string joint2 = connections[j][1]; + + int sidx = -1; + int eidx = -1; + for (size_t k = 0; k < joint_names.size(); ++k) + { + if (joint_names[k] == joint1) + { + sidx = k; + } + if (joint_names[k] == joint2) + { + eidx = k; + } + } + if (sidx == -1 || eidx == -1) + { + continue; + } + + if (poses[i][sidx][3] <= 0 || poses[i][eidx][3] <= 0) + { + continue; + } + + geometry_msgs::msg::Point p1, p2; + p1.x = poses[i][sidx][0]; + p1.y = poses[i][sidx][1]; + p1.z = poses[i][sidx][2]; + p2.x = poses[i][eidx][0]; + p2.y = poses[i][eidx][1]; + p2.z = poses[i][eidx][2]; + + marker.points.push_back(p1); + marker.points.push_back(p2); + marker.colors.push_back(color); + marker.colors.push_back(color); + } + } + + if (num_bodies == 0) + { + // Create an invisible line to remove any old skeletons + std_msgs::msg::ColorRGBA color; + color.r = 0.0; + color.g = 0.0; + color.b = 0.0; + color.a = 0.0; + + geometry_msgs::msg::Point p1, p2; + p1.x = 0.0; + p1.y = 0.0; + p1.z = 0.0; + p2.x = 0.0; + p2.y = 0.0; + p2.z = 0.001; + + marker.points.push_back(p1); + marker.points.push_back(p2); + marker.colors.push_back(color); + marker.colors.push_back(color); + } + + marker_array.markers.push_back(marker); + return marker_array; +} + +// ================================================================================================= +// ================================================================================================= + +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/extras/ros/transform_publisher/src/skeleton_tfs.cpp b/extras/ros/marker_publishers/src/skeleton_tfs.cpp similarity index 100% rename from extras/ros/transform_publisher/src/skeleton_tfs.cpp rename to extras/ros/marker_publishers/src/skeleton_tfs.cpp