Drop rotation option again.
This commit is contained in:
@ -135,7 +135,7 @@ Tested with a _Jetson AGX Orin Developer Kit_ module.
|
|||||||
- Run and test:
|
- Run and test:
|
||||||
|
|
||||||
```bash
|
```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
|
docker exec -it jetson-test_node-1 bash
|
||||||
export ROS_DOMAIN_ID=18
|
export ROS_DOMAIN_ID=18
|
||||||
|
|||||||
@ -33,7 +33,6 @@ services:
|
|||||||
- /dev/shm:/dev/shm
|
- /dev/shm:/dev/shm
|
||||||
environment:
|
environment:
|
||||||
- CAMID
|
- CAMID
|
||||||
- ROTATE
|
|
||||||
- DISPLAY
|
- DISPLAY
|
||||||
- QT_X11_NO_MITSHM=1
|
- QT_X11_NO_MITSHM=1
|
||||||
- "PYTHONUNBUFFERED=1"
|
- "PYTHONUNBUFFERED=1"
|
||||||
|
|||||||
@ -15,7 +15,7 @@ Run pose estimator with ros topics as inputs and publish detected poses.
|
|||||||
- Run and test:
|
- Run and test:
|
||||||
|
|
||||||
```bash
|
```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
|
xhost + && docker compose -f extras/ros/docker-compose-3d.yml up
|
||||||
|
|
||||||
docker exec -it ros-test_node-1 bash
|
docker exec -it ros-test_node-1 bash
|
||||||
|
|||||||
@ -33,7 +33,6 @@ services:
|
|||||||
- /dev/shm:/dev/shm
|
- /dev/shm:/dev/shm
|
||||||
environment:
|
environment:
|
||||||
- CAMID
|
- CAMID
|
||||||
- ROTATE
|
|
||||||
- DISPLAY
|
- DISPLAY
|
||||||
- QT_X11_NO_MITSHM=1
|
- QT_X11_NO_MITSHM=1
|
||||||
- "PYTHONUNBUFFERED=1"
|
- "PYTHONUNBUFFERED=1"
|
||||||
|
|||||||
@ -1,8 +1,6 @@
|
|||||||
#include <atomic>
|
#include <atomic>
|
||||||
#include <chrono>
|
#include <chrono>
|
||||||
#include <cmath>
|
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
#include <memory>
|
|
||||||
#include <string>
|
#include <string>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
|
||||||
@ -43,7 +41,7 @@ static const std::map<std::string, bool> whole_body = {
|
|||||||
class Rpt2DWrapperNode : public rclcpp::Node
|
class Rpt2DWrapperNode : public rclcpp::Node
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
Rpt2DWrapperNode(const std::string &cam_id, const std::string &rotate)
|
Rpt2DWrapperNode(const std::string &cam_id)
|
||||||
: Node("rpt2d_wrapper_" + cam_id)
|
: Node("rpt2d_wrapper_" + cam_id)
|
||||||
{
|
{
|
||||||
this->is_busy = false;
|
this->is_busy = false;
|
||||||
@ -52,14 +50,6 @@ public:
|
|||||||
std::string pose_topic = std::string(pose_out_topic)
|
std::string pose_topic = std::string(pose_out_topic)
|
||||||
.replace(pose_out_topic.find("{}"), 2, cam_id);
|
.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
|
// QoS
|
||||||
rclcpp::QoS qos_profile(1);
|
rclcpp::QoS qos_profile(1);
|
||||||
qos_profile.reliable();
|
qos_profile.reliable();
|
||||||
@ -85,7 +75,6 @@ private:
|
|||||||
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
|
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
|
||||||
rclcpp::Publisher<rpt_msgs::msg::Poses>::SharedPtr pose_pub_;
|
rclcpp::Publisher<rpt_msgs::msg::Poses>::SharedPtr pose_pub_;
|
||||||
std::atomic<bool> is_busy;
|
std::atomic<bool> is_busy;
|
||||||
int rotate_deg;
|
|
||||||
|
|
||||||
// Pose model pointer
|
// Pose model pointer
|
||||||
std::unique_ptr<utils_2d_pose::PosePredictor> kpt_model;
|
std::unique_ptr<utils_2d_pose::PosePredictor> kpt_model;
|
||||||
@ -197,24 +186,6 @@ std::vector<std::vector<std::array<float, 3>>> Rpt2DWrapperNode::call_model(cons
|
|||||||
cv::Mat rgb_image = utils_pipeline::bayer2rgb(image);
|
cv::Mat rgb_image = utils_pipeline::bayer2rgb(image);
|
||||||
std::vector<cv::Mat> images_2d = {rgb_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
|
// Predict 2D poses
|
||||||
auto poses_2d_all = kpt_model->predict(images_2d);
|
auto poses_2d_all = kpt_model->predict(images_2d);
|
||||||
auto poses_2d_upd = utils_pipeline::update_keypoints(
|
auto poses_2d_upd = utils_pipeline::update_keypoints(
|
||||||
@ -247,54 +218,6 @@ 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;
|
return valid_poses;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -304,11 +227,9 @@ std::vector<std::vector<std::array<float, 3>>> Rpt2DWrapperNode::call_model(cons
|
|||||||
int main(int argc, char **argv)
|
int main(int argc, char **argv)
|
||||||
{
|
{
|
||||||
rclcpp::init(argc, argv);
|
rclcpp::init(argc, argv);
|
||||||
|
|
||||||
const std::string cam_id = std::getenv("CAMID");
|
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);
|
auto node = std::make_shared<Rpt2DWrapperNode>(cam_id);
|
||||||
rclcpp::spin(node);
|
rclcpp::spin(node);
|
||||||
|
|
||||||
rclcpp::shutdown();
|
rclcpp::shutdown();
|
||||||
|
|||||||
Reference in New Issue
Block a user