Configurable input topics.

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

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"

extras/ros/README.md

@@ -5,6 +5,7 @@ Run pose estimator with ros topics as inputs and publish detected poses.
 <br>
 
 - 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

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'

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 = "/{}/pylon_ros2_camera_node/image_raw"
-img_input_topic = "/" + cam_id + "/pylon_ros2_camera_node/image_raw"
+pose_input_topic = "/poses/{}"
-pose_input_topic = "/poses/" + cam_id
+img_output_topic = "/{}/img_with_pose"
-img_output_topic = "/" + cam_id + "/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")
 

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 = "/{}/pylon_ros2_camera_node/image_raw";
-static const std::string img_input_topic = "/" + cam_id + "/pylon_ros2_camera_node/image_raw";
+static const std::string pose_out_topic = "/poses/{}";
-static const std::string pose_out_topic = "/poses/" + cam_id;
 
 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<std::string, bool> whole_body = {
 class Rpt2DWrapperNode : public rclcpp::Node
 {
 public:
-    Rpt2DWrapperNode(const std::string &node_name)
+    Rpt2DWrapperNode(const std::string &cam_id, const std::string &rotate)
-        : Node(node_name)
+        : 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<sensor_msgs::msg::Image>(
-            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<rpt_msgs::msg::Poses>(pose_out_topic, qos_profile);
+        pose_pub_ = this->create_publisher<rpt_msgs::msg::Poses>(pose_topic, qos_profile);
 
         // Load model
         bool use_wb = utils_pipeline::use_whole_body(whole_body);
@@ -74,6 +85,7 @@ private:
     rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
     rclcpp::Publisher<rpt_msgs::msg::Poses>::SharedPtr pose_pub_;
     std::atomic<bool> is_busy;
+    int rotate_deg;
 
     // Pose model pointer
     std::unique_ptr<utils_2d_pose::PosePredictor> kpt_model;
@@ -153,11 +165,11 @@ void Rpt2DWrapperNode::callback_images(const sensor_msgs::msg::Image::SharedPtr
     std::vector<int32_t> 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<std::vector<std::array<float, 3>>> Rpt2DWrapperNode::call_model(cons
     cv::Mat rgb_image = utils_pipeline::bayer2rgb(image);
     std::vector<cv::Mat> 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<std::vector<std::array<float, 3>>> 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<Rpt2DWrapperNode>("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<Rpt2DWrapperNode>(cam_id, rotate);
     rclcpp::spin(node);
 
     rclcpp::shutdown();