crosstyan/RapidPoseTriangulation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Configurable input topics.

Browse Source
This commit is contained in:
Daniel
2025-02-17 18:54:59 +01:00
parent 7ce02c44c8
commit 6fbf279334
6 changed files with 116 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
extras/ros/README.md
View File

@ -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

8
extras/ros/docker-compose.yml
View File

@ -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'

18
extras/ros/pose2d_visualizer/pose2d_visualizer/pose2d_visualizer.py
View File

@ -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")

103
extras/ros/rpt2d_wrapper_cpp/src/rpt2d_wrapper.cpp
View File

@ -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<std::string, bool> 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<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();