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

Added rpt3d ros wrapper.

Browse Source
This commit is contained in:
Daniel
2025-02-18 12:27:32 +01:00
parent 6fbf279334
commit 483df4fc3e
10 changed files with 395 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
extras/ros/README.md
View File

@ -15,7 +15,8 @@ Run pose estimator with ros topics as inputs and publish detected poses.
- Run and test:
```bash
xhost + && export CAMID="camera01" && export ROTATE="0" && docker compose -f extras/ros/docker-compose.yml up
xhost + && export CAMID="camera01" && export ROTATE="0" && docker compose -f extras/ros/docker-compose-2d.yml up
xhost + && docker compose -f extras/ros/docker-compose-3d.yml up
docker exec -it ros-test_node-1 bash
export ROS_DOMAIN_ID=18

0
extras/ros/docker-compose.yml → extras/ros/docker-compose-2d.yml
View File

38
extras/ros/docker-compose-3d.yml Normal file
View File

@ -0,0 +1,38 @@
version: "2.3"
# runtime: nvidia needs version 2 else change standard runtime at host pc
services:
test_node:
image: rapidposetriangulation_ros
network_mode: "host"
ipc: "host"
runtime: nvidia
privileged: true
volumes:
- ../../:/RapidPoseTriangulation/
- ../../skelda/:/skelda/
- /tmp/.X11-unix:/tmp/.X11-unix
- /dev/shm:/dev/shm
environment:
- DISPLAY
- QT_X11_NO_MITSHM=1
- "PYTHONUNBUFFERED=1"
command: /bin/bash -i -c 'sleep infinity'
triangulator:
image: rapidposetriangulation_ros
network_mode: "host"
ipc: "host"
runtime: nvidia
privileged: true
volumes:
- ../../:/RapidPoseTriangulation/
- ../../skelda/:/skelda/
- /tmp/.X11-unix:/tmp/.X11-unix
- /dev/shm:/dev/shm
environment:
- DISPLAY
- QT_X11_NO_MITSHM=1
- "PYTHONUNBUFFERED=1"
command: /bin/bash -i -c 'export ROS_DOMAIN_ID=18 && ros2 run rpt3d_wrapper_cpp rpt3d_wrapper'

3
extras/ros/dockerfile
View File

@ -45,14 +45,17 @@ RUN echo "source /project/dev_ws/install/setup.bash" >> ~/.bashrc
# Copy modules
COPY ./extras/include/ /RapidPoseTriangulation/extras/include/
COPY ./scripts/ /RapidPoseTriangulation/scripts/
COPY ./rpt/ /RapidPoseTriangulation/rpt/
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/
COPY ./extras/ros/rpt3d_wrapper_cpp/ /RapidPoseTriangulation/extras/ros/rpt3d_wrapper_cpp/
# Link and build as ros package
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 ln -s /RapidPoseTriangulation/extras/ros/rpt3d_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

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

@ -154,10 +154,10 @@ void Rpt2DWrapperNode::callback_images(const sensor_msgs::msg::Image::SharedPtr
jdata["timestamps"] = {
{"trigger", time_stamp},
{"image", ts_image_sec},
{"pose", ts_pose_sec},
{"pose2d", ts_pose_sec},
{"z-trigger-image", z_trigger_image},
{"z-image-pose", z_image_pose},
{"z-trigger-pose", z_trigger_pose}};
{"z-image-pose2d", z_image_pose},
{"z-trigger-pose2d", z_trigger_pose}};
// Publish message
auto pose_msg = rpt_msgs::msg::Poses();

72
extras/ros/rpt3d_wrapper_cpp/CMakeLists.txt Normal file
View File

@ -0,0 +1,72 @@
cmake_minimum_required(VERSION 3.5)
project(rpt3d_wrapper_cpp)
# Default to C99
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
# Default to C++17
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
endif()
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(rclcpp REQUIRED)
find_package(rpt_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(OpenCV REQUIRED)
find_package(OpenMP REQUIRED)
# Add RapidPoseTriangulation implementation
set(RAPID_POSE_TRIANGULATION_DIR "/RapidPoseTriangulation")
add_library(rapid_pose_triangulation
${RAPID_POSE_TRIANGULATION_DIR}/rpt/camera.cpp
${RAPID_POSE_TRIANGULATION_DIR}/rpt/interface.cpp
${RAPID_POSE_TRIANGULATION_DIR}/rpt/triangulator.cpp
)
target_include_directories(rapid_pose_triangulation PUBLIC
${RAPID_POSE_TRIANGULATION_DIR}/extras/include
${RAPID_POSE_TRIANGULATION_DIR}/rpt
${OpenCV_INCLUDE_DIRS}
)
target_link_libraries(rapid_pose_triangulation PUBLIC
${OpenCV_LIBS}
OpenMP::OpenMP_CXX
)
target_compile_options(rapid_pose_triangulation PUBLIC
-fPIC -O3 -march=native -Wall -Werror -fopenmp -fopenmp-simd
)
# Build the executable
add_executable(rpt3d_wrapper src/rpt3d_wrapper.cpp)
ament_target_dependencies(rpt3d_wrapper rclcpp std_msgs rpt_msgs)
target_include_directories(rpt3d_wrapper PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
target_link_libraries(rpt3d_wrapper
${OpenCV_LIBS}
rapid_pose_triangulation
)
install(TARGETS rpt3d_wrapper
DESTINATION lib/${PROJECT_NAME})
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# uncomment the line when a copyright and license is not present in all source files
#set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# uncomment the line when this package is not in a git repo
#set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()

24
extras/ros/rpt3d_wrapper_cpp/package.xml Normal file
View File

@ -0,0 +1,24 @@
<?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>rpt3d_wrapper_cpp</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="root@todo.todo">root</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>rpt_msgs</depend>
<depend>std_msgs</depend>
<depend>OpenCV</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

252
extras/ros/rpt3d_wrapper_cpp/src/rpt3d_wrapper.cpp Normal file
View File

@ -0,0 +1,252 @@
#include <atomic>
#include <chrono>
#include <iostream>
#include <string>
#include <vector>
// ROS2
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
// JSON library
#include "/RapidPoseTriangulation/extras/include/nlohmann/json.hpp"
using json = nlohmann::json;
#include "rpt_msgs/msg/poses.hpp"
#include "/RapidPoseTriangulation/rpt/camera.hpp"
#include "/RapidPoseTriangulation/rpt/interface.hpp"
// =================================================================================================
static const std::vector<std::string> cam_ids = {
"camera01",
"camera02",
"camera03",
"camera04",
"camera05",
"camera06",
"camera07",
"camera08",
"camera09",
"camera10",
};
static const std::string pose_in_topic = "/poses/{}";
static const std::string cam_info_topic = "/{}/calibration";
static const std::string pose_out_topic = "/poses/humans3d";
static const float min_match_score = 0.95;
static const size_t min_group_size = 1;
static const std::array<std::array<float, 3>, 2> roomparams = {{
{4.0, 5.0, 2.2},
{1.0, 0.0, 1.1},
}};
// =================================================================================================
// =================================================================================================
class Rpt3DWrapperNode : public rclcpp::Node
{
public:
Rpt3DWrapperNode()
: Node("rpt3d_wrapper")
{
this->all_cams_set = false;
this->cameras.resize(cam_ids.size());
this->all_poses.resize(cam_ids.size());
this->all_timestamps.resize(cam_ids.size());
this->all_poses_set.resize(cam_ids.size(), false);
// Load 3D model
tri_model = std::make_unique<Triangulator>(
min_match_score, min_group_size);
// QoS
rclcpp::QoS qos_profile(1);
qos_profile.reliable();
qos_profile.keep_last(1);
// Setup subscribers
for (size_t i = 0; i < cam_ids.size(); i++)
{
std::string cam_id = cam_ids[i];
std::string topic_pose = pose_in_topic;
std::string topic_cam = cam_info_topic;
topic_pose.replace(topic_pose.find("{}"), 2, cam_id);
topic_cam.replace(topic_cam.find("{}"), 2, cam_id);
auto sub_pose = this->create_subscription<rpt_msgs::msg::Poses>(
topic_pose, qos_profile,
[this, i](const rpt_msgs::msg::Poses::SharedPtr msg)
{
this->callback_poses(msg, i);
});
sub_pose_list_.push_back(sub_pose);
auto sub_cam = this->create_subscription<std_msgs::msg::String>(
topic_cam, qos_profile,
[this, i](const std_msgs::msg::String::SharedPtr msg)
{
this->callback_cam_info(msg, i);
});
sub_cam_list_.push_back(sub_cam);
}
// Setup publisher
pose_pub_ = this->create_publisher<rpt_msgs::msg::Poses>(pose_out_topic, qos_profile);
RCLCPP_INFO(this->get_logger(), "Finished initialization of pose triangulator.");
}
private:
std::vector<rclcpp::Subscription<rpt_msgs::msg::Poses>::SharedPtr> sub_pose_list_;
std::vector<rclcpp::Subscription<std_msgs::msg::String>::SharedPtr> sub_cam_list_;
rclcpp::Publisher<rpt_msgs::msg::Poses>::SharedPtr pose_pub_;
std::unique_ptr<Triangulator> tri_model;
std::vector<Camera> cameras;
std::atomic<bool> all_cams_set;
std::vector<std::vector<std::vector<std::array<float, 3>>>> all_poses;
std::vector<double> all_timestamps;
std::vector<bool> all_poses_set;
void callback_poses(const rpt_msgs::msg::Poses::SharedPtr msg, size_t cam_idx);
void callback_cam_info(const std_msgs::msg::String::SharedPtr msg, size_t cam_idx);
};
// =================================================================================================
void Rpt3DWrapperNode::callback_cam_info(const std_msgs::msg::String::SharedPtr msg, size_t cam_idx)
{
json cam = json::parse(msg->data);
Camera camera;
camera.name = cam["name"].get<std::string>();
camera.K = cam["K"].get<std::array<std::array<float, 3>, 3>>();
camera.DC = cam["DC"].get<std::vector<float>>();
camera.R = cam["R"].get<std::array<std::array<float, 3>, 3>>();
camera.T = cam["T"].get<std::array<std::array<float, 1>, 3>>();
camera.width = cam["width"].get<int>();
camera.height = cam["height"].get<int>();
camera.type = cam["type"].get<std::string>();
cameras[cam_idx] = camera;
// Check if all cameras are set
bool all_set = true;
for (size_t i = 0; i < cam_ids.size(); i++)
{
if (cameras[i].name.empty())
{
all_set = false;
break;
}
}
this->all_cams_set = all_set;
}
// =================================================================================================
void Rpt3DWrapperNode::callback_poses(const rpt_msgs::msg::Poses::SharedPtr msg, size_t cam_idx)
{
if (!this->all_cams_set)
{
RCLCPP_WARN(this->get_logger(), "Skipping frame, still waiting for cameras...");
return;
}
auto ts_image = std::chrono::high_resolution_clock::now();
std::vector<std::vector<std::array<float, 3>>> poses;
std::vector<std::string> &joint_names_2d = msg->joint_names;
// Unflatten poses
size_t idx = 0;
std::vector<int32_t> &bshape = msg->bodies_shape;
for (int32_t i = 0; i < bshape[0]; ++i)
{
std::vector<std::array<float, 3>> body;
for (int32_t j = 0; j < bshape[1]; ++j)
{
std::array<float, 3> 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));
}
this->all_poses[cam_idx] = std::move(poses);
this->all_poses_set[cam_idx] = true;
this->all_timestamps[cam_idx] = msg->header.stamp.sec + msg->header.stamp.nanosec / 1.0e9;
// If not all poses are set, return and wait for the others
for (size_t i = 0; i < cam_ids.size(); i++)
{
if (!this->all_poses_set[i])
{
return;
}
}
// Call model
const auto valid_poses = tri_model->triangulate_poses(
all_poses, cameras, roomparams, joint_names_2d);
// Calculate timings
double min_ts = *std::min_element(all_timestamps.begin(), all_timestamps.end());
auto ts_pose = std::chrono::high_resolution_clock::now();
double ts_pose_sec = std::chrono::duration<double>(ts_pose.time_since_epoch()).count();
double z_trigger_pose3d = ts_pose_sec - min_ts;
json jdata;
jdata["timestamps"] = {
{"trigger", min_ts},
{"pose3d", ts_pose_sec},
{"z-trigger-pose3d", z_trigger_pose3d}};
// Publish message
auto pose_msg = rpt_msgs::msg::Poses();
pose_msg.header = msg->header;
pose_msg.header.stamp.sec = static_cast<int>(min_ts);
pose_msg.header.stamp.nanosec = (min_ts - pose_msg.header.stamp.sec) * 1.0e9;
std::vector<int32_t> pshape = {(int)valid_poses.size(), (int)joint_names_2d.size(), 4};
pose_msg.bodies_shape = bshape;
pose_msg.bodies_flat.reserve(pshape[0] * pshape[1] * pshape[2]);
for (int32_t i = 0; i < pshape[0]; i++)
{
for (int32_t j = 0; j < pshape[1]; j++)
{
for (int32_t k = 0; k < pshape[2]; 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>(
std::chrono::high_resolution_clock::now() - ts_image)
.count();
std::cout << "Detected persons: " << valid_poses.size()
<< " - Prediction time: " << elapsed_time << "s" << std::endl;
this->all_poses_set = std::vector<bool>(cam_ids.size(), false);
}
// =================================================================================================
// =================================================================================================
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<Rpt3DWrapperNode>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}