From 63e00e9b1352752044f8b7c2a47e8782f744ce63 Mon Sep 17 00:00:00 2001 From: Daniel Date: Thu, 20 Feb 2025 18:47:16 +0100 Subject: [PATCH] Drop rotation option again. --- extras/jetson/README.md | 2 +- extras/jetson/docker-compose-2d.yml | 1 - extras/ros/README.md | 2 +- extras/ros/docker-compose-2d.yml | 1 - .../rpt2d_wrapper_cpp/src/rpt2d_wrapper.cpp | 83 +------------------ 5 files changed, 4 insertions(+), 85 deletions(-) diff --git a/extras/jetson/README.md b/extras/jetson/README.md index 38f8ef3..9607e43 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-2d.yml up + export CAMID="camera01" && 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-2d.yml b/extras/jetson/docker-compose-2d.yml index a4a3709..64f55be 100644 --- a/extras/jetson/docker-compose-2d.yml +++ b/extras/jetson/docker-compose-2d.yml @@ -33,7 +33,6 @@ services: - /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 cfad584..dda355f 100644 --- a/extras/ros/README.md +++ b/extras/ros/README.md @@ -15,7 +15,7 @@ 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-2d.yml up + xhost + && export CAMID="camera01" && 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 diff --git a/extras/ros/docker-compose-2d.yml b/extras/ros/docker-compose-2d.yml index 43eb0df..2501950 100644 --- a/extras/ros/docker-compose-2d.yml +++ b/extras/ros/docker-compose-2d.yml @@ -33,7 +33,6 @@ services: - /dev/shm:/dev/shm environment: - CAMID - - ROTATE - DISPLAY - QT_X11_NO_MITSHM=1 - "PYTHONUNBUFFERED=1" diff --git a/extras/ros/rpt2d_wrapper_cpp/src/rpt2d_wrapper.cpp b/extras/ros/rpt2d_wrapper_cpp/src/rpt2d_wrapper.cpp index 8fffb98..0d2ed12 100644 --- a/extras/ros/rpt2d_wrapper_cpp/src/rpt2d_wrapper.cpp +++ b/extras/ros/rpt2d_wrapper_cpp/src/rpt2d_wrapper.cpp @@ -1,8 +1,6 @@ #include #include -#include #include -#include #include #include @@ -43,7 +41,7 @@ static const std::map whole_body = { class Rpt2DWrapperNode : public rclcpp::Node { public: - Rpt2DWrapperNode(const std::string &cam_id, const std::string &rotate) + Rpt2DWrapperNode(const std::string &cam_id) : Node("rpt2d_wrapper_" + cam_id) { this->is_busy = false; @@ -52,14 +50,6 @@ public: 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); qos_profile.reliable(); @@ -85,7 +75,6 @@ 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; @@ -197,24 +186,6 @@ 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( @@ -247,54 +218,6 @@ 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; } @@ -304,11 +227,9 @@ std::vector>> Rpt2DWrapperNode::call_model(cons int main(int argc, char **argv) { rclcpp::init(argc, argv); - 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); + auto node = std::make_shared(cam_id); rclcpp::spin(node); rclcpp::shutdown();