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();