diff --git a/extras/jetson/README.md b/extras/jetson/README.md index e81ef4e..38f8ef3 100644 --- a/extras/jetson/README.md +++ b/extras/jetson/README.md @@ -135,7 +135,7 @@ Tested with a _Jetson AGX Orin Developer Kit_ module. - Run and test: ```bash - export CAMID="camera01" && export ROTATE="0" && docker compose -f extras/jetson/docker-compose.yml up + export CAMID="camera01" && export ROTATE="0" && docker compose -f extras/jetson/docker-compose-2d.yml up docker exec -it jetson-test_node-1 bash export ROS_DOMAIN_ID=18 diff --git a/extras/jetson/docker-compose.yml b/extras/jetson/docker-compose-2d.yml similarity index 100% rename from extras/jetson/docker-compose.yml rename to extras/jetson/docker-compose-2d.yml diff --git a/extras/ros/README.md b/extras/ros/README.md index 8640f42..cfad584 100644 --- a/extras/ros/README.md +++ b/extras/ros/README.md @@ -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 diff --git a/extras/ros/docker-compose.yml b/extras/ros/docker-compose-2d.yml similarity index 100% rename from extras/ros/docker-compose.yml rename to extras/ros/docker-compose-2d.yml diff --git a/extras/ros/docker-compose-3d.yml b/extras/ros/docker-compose-3d.yml new file mode 100644 index 0000000..f567b87 --- /dev/null +++ b/extras/ros/docker-compose-3d.yml @@ -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' diff --git a/extras/ros/dockerfile b/extras/ros/dockerfile index fe72c48..3def5aa 100644 --- a/extras/ros/dockerfile +++ b/extras/ros/dockerfile @@ -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 diff --git a/extras/ros/rpt2d_wrapper_cpp/src/rpt2d_wrapper.cpp b/extras/ros/rpt2d_wrapper_cpp/src/rpt2d_wrapper.cpp index 283ad20..8fffb98 100644 --- a/extras/ros/rpt2d_wrapper_cpp/src/rpt2d_wrapper.cpp +++ b/extras/ros/rpt2d_wrapper_cpp/src/rpt2d_wrapper.cpp @@ -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(); diff --git a/extras/ros/rpt3d_wrapper_cpp/CMakeLists.txt b/extras/ros/rpt3d_wrapper_cpp/CMakeLists.txt new file mode 100644 index 0000000..303809a --- /dev/null +++ b/extras/ros/rpt3d_wrapper_cpp/CMakeLists.txt @@ -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 + $ + $) + +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() diff --git a/extras/ros/rpt3d_wrapper_cpp/package.xml b/extras/ros/rpt3d_wrapper_cpp/package.xml new file mode 100644 index 0000000..953455d --- /dev/null +++ b/extras/ros/rpt3d_wrapper_cpp/package.xml @@ -0,0 +1,24 @@ + + + + rpt3d_wrapper_cpp + 0.0.0 + TODO: Package description + root + TODO: License declaration + + ament_cmake + + rclcpp + rpt_msgs + std_msgs + + OpenCV + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/extras/ros/rpt3d_wrapper_cpp/src/rpt3d_wrapper.cpp b/extras/ros/rpt3d_wrapper_cpp/src/rpt3d_wrapper.cpp new file mode 100644 index 0000000..e902232 --- /dev/null +++ b/extras/ros/rpt3d_wrapper_cpp/src/rpt3d_wrapper.cpp @@ -0,0 +1,252 @@ +#include +#include +#include +#include +#include + +// ROS2 +#include +#include + +// 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 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, 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( + 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( + 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( + 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(pose_out_topic, qos_profile); + + RCLCPP_INFO(this->get_logger(), "Finished initialization of pose triangulator."); + } + +private: + std::vector::SharedPtr> sub_pose_list_; + std::vector::SharedPtr> sub_cam_list_; + rclcpp::Publisher::SharedPtr pose_pub_; + + std::unique_ptr tri_model; + std::vector cameras; + std::atomic all_cams_set; + + std::vector>>> all_poses; + std::vector all_timestamps; + std::vector 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(); + camera.K = cam["K"].get, 3>>(); + camera.DC = cam["DC"].get>(); + camera.R = cam["R"].get, 3>>(); + camera.T = cam["T"].get, 3>>(); + camera.width = cam["width"].get(); + camera.height = cam["height"].get(); + camera.type = cam["type"].get(); + 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>> poses; + std::vector &joint_names_2d = 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)); + } + + 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(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(min_ts); + pose_msg.header.stamp.nanosec = (min_ts - pose_msg.header.stamp.sec) * 1.0e9; + std::vector 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( + 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(cam_ids.size(), false); +} + +// ================================================================================================= +// ================================================================================================= + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +}