From 6fbf2793343cad85d3f40523be34a111cd239b16 Mon Sep 17 00:00:00 2001 From: Daniel Date: Mon, 17 Feb 2025 18:54:59 +0100 Subject: [PATCH] Configurable input topics. --- extras/jetson/README.md | 3 +- extras/jetson/docker-compose.yml | 2 + extras/ros/README.md | 4 +- extras/ros/docker-compose.yml | 8 +- .../pose2d_visualizer/pose2d_visualizer.py | 18 +-- .../rpt2d_wrapper_cpp/src/rpt2d_wrapper.cpp | 103 ++++++++++++++++-- 6 files changed, 116 insertions(+), 22 deletions(-) diff --git a/extras/jetson/README.md b/extras/jetson/README.md index cdc3a53..e81ef4e 100644 --- a/extras/jetson/README.md +++ b/extras/jetson/README.md @@ -133,8 +133,9 @@ Tested with a _Jetson AGX Orin Developer Kit_ module. ``` - Run and test: + ```bash - docker compose -f extras/jetson/docker-compose.yml up + export CAMID="camera01" && export ROTATE="0" && docker compose -f extras/jetson/docker-compose.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.yml index 9409e6e..a4a3709 100644 --- a/extras/jetson/docker-compose.yml +++ b/extras/jetson/docker-compose.yml @@ -32,6 +32,8 @@ services: - /tmp/.X11-unix:/tmp/.X11-unix - /dev/shm:/dev/shm environment: + - CAMID + - ROTATE - DISPLAY - QT_X11_NO_MITSHM=1 - "PYTHONUNBUFFERED=1" diff --git a/extras/ros/README.md b/extras/ros/README.md index 1c8a869..8640f42 100644 --- a/extras/ros/README.md +++ b/extras/ros/README.md @@ -5,6 +5,7 @@ Run pose estimator with ros topics as inputs and publish detected poses.
- Build container: + ```bash docker build --progress=plain -t rapidposetriangulation_ros -f extras/ros/dockerfile . ``` @@ -12,8 +13,9 @@ Run pose estimator with ros topics as inputs and publish detected poses. - Update or remove the `ROS_DOMAIN_ID` in `docker-compose.yml` to fit your environment - Run and test: + ```bash - xhost +; docker compose -f extras/ros/docker-compose.yml up + xhost + && export CAMID="camera01" && export ROTATE="0" && docker compose -f extras/ros/docker-compose.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.yml index 507f011..43eb0df 100644 --- a/extras/ros/docker-compose.yml +++ b/extras/ros/docker-compose.yml @@ -32,6 +32,8 @@ services: - /tmp/.X11-unix:/tmp/.X11-unix - /dev/shm:/dev/shm environment: + - CAMID + - ROTATE - DISPLAY - QT_X11_NO_MITSHM=1 - "PYTHONUNBUFFERED=1" @@ -49,10 +51,11 @@ services: - /tmp/.X11-unix:/tmp/.X11-unix - /dev/shm:/dev/shm environment: + - CAMID - DISPLAY - QT_X11_NO_MITSHM=1 - "PYTHONUNBUFFERED=1" - command: /bin/bash -i -c 'sleep 2 && export ROS_DOMAIN_ID=18 && ros2 run pose2d_visualizer pose2d_visualizer' + command: /bin/bash -i -c 'sleep 2 && export ROS_DOMAIN_ID=18 && ros2 run pose2d_visualizer pose2d_visualizer $CAMID' pose_viewer: image: rapidposetriangulation_ros @@ -64,7 +67,8 @@ services: - /tmp/.X11-unix:/tmp/.X11-unix - /dev/shm:/dev/shm environment: + - CAMID - DISPLAY - QT_X11_NO_MITSHM=1 - "PYTHONUNBUFFERED=1" - command: /bin/bash -i -c 'sleep 2 && export ROS_DOMAIN_ID=18 && ros2 run image_view image_view --ros-args --remap image:=/camera01/img_with_pose -p autosize:=True -p window_name:=MyPoseImage' + command: /bin/bash -i -c 'sleep 2 && export ROS_DOMAIN_ID=18 && ros2 run image_view image_view --ros-args --remap image:=/$CAMID/img_with_pose -p autosize:=True -p window_name:=MyPoseImage' diff --git a/extras/ros/pose2d_visualizer/pose2d_visualizer/pose2d_visualizer.py b/extras/ros/pose2d_visualizer/pose2d_visualizer/pose2d_visualizer.py index 7e2a9b2..7c717b2 100644 --- a/extras/ros/pose2d_visualizer/pose2d_visualizer/pose2d_visualizer.py +++ b/extras/ros/pose2d_visualizer/pose2d_visualizer/pose2d_visualizer.py @@ -19,10 +19,9 @@ bridge = CvBridge() node = None publisher_img = None -cam_id = "camera01" -img_input_topic = "/" + cam_id + "/pylon_ros2_camera_node/image_raw" -pose_input_topic = "/poses/" + cam_id -img_output_topic = "/" + cam_id + "/img_with_pose" +img_input_topic = "/{}/pylon_ros2_camera_node/image_raw" +pose_input_topic = "/poses/{}" +img_output_topic = "/{}/img_with_pose" last_input_image = None lock = threading.Lock() @@ -113,6 +112,7 @@ def main(): # Start node rclpy.init(args=sys.argv) + cam_id = sys.argv[1] node = rclpy.create_node("pose2d_visualizer") # Quality of service settings @@ -125,19 +125,23 @@ def main(): # Create subscribers _ = node.create_subscription( Image, - img_input_topic, + img_input_topic.format(cam_id), callback_images, qos_profile, ) _ = node.create_subscription( Poses, - pose_input_topic, + pose_input_topic.format(cam_id), callback_poses, qos_profile, ) # Create publishers - publisher_img = node.create_publisher(Image, img_output_topic, qos_profile) + publisher_img = node.create_publisher( + Image, + img_output_topic.format(cam_id), + qos_profile, + ) node.get_logger().info("Finished initialization of pose visualizer") diff --git a/extras/ros/rpt2d_wrapper_cpp/src/rpt2d_wrapper.cpp b/extras/ros/rpt2d_wrapper_cpp/src/rpt2d_wrapper.cpp index 5307be5..283ad20 100644 --- a/extras/ros/rpt2d_wrapper_cpp/src/rpt2d_wrapper.cpp +++ b/extras/ros/rpt2d_wrapper_cpp/src/rpt2d_wrapper.cpp @@ -24,9 +24,8 @@ using json = nlohmann::json; // ================================================================================================= -static const std::string cam_id = "camera01"; -static const std::string img_input_topic = "/" + cam_id + "/pylon_ros2_camera_node/image_raw"; -static const std::string pose_out_topic = "/poses/" + cam_id; +static const std::string img_input_topic = "/{}/pylon_ros2_camera_node/image_raw"; +static const std::string pose_out_topic = "/poses/{}"; static const float min_bbox_score = 0.4; static const float min_bbox_area = 0.1 * 0.1; @@ -44,10 +43,22 @@ static const std::map whole_body = { class Rpt2DWrapperNode : public rclcpp::Node { public: - Rpt2DWrapperNode(const std::string &node_name) - : Node(node_name) + Rpt2DWrapperNode(const std::string &cam_id, const std::string &rotate) + : Node("rpt2d_wrapper_" + cam_id) { this->is_busy = false; + std::string img_topic = std::string(img_input_topic) + .replace(img_input_topic.find("{}"), 2, cam_id); + std::string pose_topic = std::string(pose_out_topic) + .replace(pose_out_topic.find("{}"), 2, cam_id); + + // Check rotation + if (rotate != "0" && rotate != "90" && rotate != "180" && rotate != "-90") + { + RCLCPP_ERROR(this->get_logger(), "Invalid rotation value: %s", rotate.c_str()); + return; + } + this->rotate_deg = std::stoi(rotate); // QoS rclcpp::QoS qos_profile(1); @@ -56,11 +67,11 @@ public: // Setup subscriber image_sub_ = this->create_subscription( - img_input_topic, qos_profile, + img_topic, qos_profile, 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_topic, qos_profile); // Load model bool use_wb = utils_pipeline::use_whole_body(whole_body); @@ -74,6 +85,7 @@ private: rclcpp::Subscription::SharedPtr image_sub_; rclcpp::Publisher::SharedPtr pose_pub_; std::atomic is_busy; + int rotate_deg; // Pose model pointer std::unique_ptr kpt_model; @@ -153,11 +165,11 @@ void Rpt2DWrapperNode::callback_images(const sensor_msgs::msg::Image::SharedPtr std::vector bshape = {(int)valid_poses.size(), (int)joint_names_2d.size(), 3}; pose_msg.bodies_shape = bshape; pose_msg.bodies_flat.reserve(bshape[0] * bshape[1] * bshape[2]); - for (size_t i = 0; i < bshape[0]; i++) + for (int32_t i = 0; i < bshape[0]; i++) { - for (size_t j = 0; j < bshape[1]; j++) + for (int32_t j = 0; j < bshape[1]; j++) { - for (size_t k = 0; k < bshape[2]; k++) + for (int32_t k = 0; k < bshape[2]; k++) { pose_msg.bodies_flat.push_back(valid_poses[i][j][k]); } @@ -185,6 +197,24 @@ std::vector>> Rpt2DWrapperNode::call_model(cons cv::Mat rgb_image = utils_pipeline::bayer2rgb(image); std::vector images_2d = {rgb_image}; + // Optionally rotate image + if (this->rotate_deg == 0) + { + // pass + } + else if (this->rotate_deg == 90) + { + cv::rotate(images_2d[0], images_2d[0], cv::ROTATE_90_CLOCKWISE); + } + else if (this->rotate_deg == 180) + { + cv::rotate(images_2d[0], images_2d[0], cv::ROTATE_180); + } + else if (this->rotate_deg == -90) + { + cv::rotate(images_2d[0], images_2d[0], cv::ROTATE_90_COUNTERCLOCKWISE); + } + // Predict 2D poses auto poses_2d_all = kpt_model->predict(images_2d); auto poses_2d_upd = utils_pipeline::update_keypoints( @@ -217,6 +247,54 @@ std::vector>> Rpt2DWrapperNode::call_model(cons } } + // Rotate poses back + float img_width = image.cols; + float img_height = image.rows; + if (this->rotate_deg == 0) + { + // pass + } + else if (this->rotate_deg == 90) + { + for (auto &person : valid_poses) + { + for (auto &kp : person) + { + float x = kp[0]; + float y = kp[1]; + kp[0] = y; + kp[1] = img_height - 1 - x; + } + } + } + else if (this->rotate_deg == 180) + { + for (auto &person : valid_poses) + { + for (auto &kp : person) + { + float x = kp[0]; + float y = kp[1]; + kp[0] = img_width - 1 - x; + kp[1] = img_height - 1 - y; + } + } + } + else if (this->rotate_deg == -90) + { + for (auto &person : valid_poses) + { + for (auto &kp : person) + { + float x = kp[0]; + float y = kp[1]; + kp[0] = img_width - 1 - y; + kp[1] = x; + + } + } + } + return valid_poses; } @@ -227,7 +305,10 @@ int main(int argc, char **argv) { rclcpp::init(argc, argv); - auto node = std::make_shared("rpt2d_wrapper"); + const std::string cam_id = std::getenv("CAMID"); + const std::string rotate = std::getenv("ROTATE") ? std::getenv("ROTATE") : "0"; + + auto node = std::make_shared(cam_id, rotate); rclcpp::spin(node); rclcpp::shutdown();