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

Some small updates.

Browse Source
This commit is contained in:
Daniel
2025-02-05 16:29:08 +01:00
parent 1fb730ff55
commit 24d706d030
4 changed files with 13 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -18,7 +18,7 @@
#include "/RapidPoseTriangulation/extras/include/nlohmann/json.hpp"
using json = nlohmann::json;
#include "rpt_msgs/msg/pose.hpp"
#include "rpt_msgs/msg/poses.hpp"
#include "/RapidPoseTriangulation/scripts/utils_2d_pose.hpp"
#include "/RapidPoseTriangulation/scripts/utils_pipeline.hpp"
@ -60,7 +60,7 @@ public:
std::bind(&Rpt2DWrapperNode::callback_images, this, std::placeholders::_1));
// Setup publisher
pose_pub_ = this->create_publisher<rpt_msgs::msg::Pose>(pose_out_topic, qos_profile);
pose_pub_ = this->create_publisher<rpt_msgs::msg::Poses>(pose_out_topic, qos_profile);
// Load model
bool use_wb = utils_pipeline::use_whole_body(whole_body);
@ -72,7 +72,7 @@ public:
private:
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
rclcpp::Publisher<rpt_msgs::msg::Pose>::SharedPtr pose_pub_;
rclcpp::Publisher<rpt_msgs::msg::Poses>::SharedPtr pose_pub_;
std::atomic<bool> is_busy;
// Pose model pointer
@ -148,17 +148,16 @@ void Rpt2DWrapperNode::callback_images(const sensor_msgs::msg::Image::SharedPtr
{"z-trigger-pose", z_trigger_pose}};
// Publish message
auto pose_msg = rpt_msgs::msg::Pose();
auto pose_msg = rpt_msgs::msg::Poses();
pose_msg.header = msg->header;
pose_msg.bodies_shape.push_back(valid_poses.size());
pose_msg.bodies_shape.push_back(joint_names_2d.size());
pose_msg.bodies_shape.push_back(3);
pose_msg.bodies_flat.reserve(valid_poses.size() * joint_names_2d.size() * 3);
for (size_t i=0; i<valid_poses.size(); i++)
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 (size_t j=0; j<joint_names_2d.size(); j++)
for (size_t j = 0; j < bshape[1]; j++)
{
for (size_t k=0; k<3; k++)
for (size_t k = 0; k < bshape[2]; k++)
{
pose_msg.bodies_flat.push_back(valid_poses[i][j][k]);
}