wanngwe/odin_ros_driver1
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add yolo + mot cpp into driver

Browse Source
This commit is contained in:
黄JY
2026-04-10 14:37:41 +08:00
parent 3659ffd110
commit 347a6a12e0
9 changed files with 721 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/cloud_reprojection_ros.cpp
+309 -89
View File
@@ -12,9 +12,14 @@ limitations under the License.
*/
#include "cloud_reprojection_ros_node.hpp"
#include "cloud_reprojection_processing.hpp"
#include <algorithm>
#include <chrono>
#include <filesystem>
#include <fstream>
#include <iomanip>
#include <sstream>
#include <sys/stat.h>
#include <thread>
#include <chrono>
@@ -23,95 +28,58 @@ limitations under the License.
#ifdef ROS2
#include <sensor_msgs/msg/compressed_image.hpp>
#include <geometry_msgs/msg/point_stamped.hpp>
#include <std_msgs/msg/float32_multi_array.hpp>
#include <std_msgs/msg/int32.hpp>
#endif
namespace {
cv::Mat resizeToHeight(const cv::Mat& src, int target_h)
std::filesystem::path get_package_source_directory_from_file()
{
if (src.empty() || target_h <= 0) {
return src.clone();
}
if (src.rows == target_h) {
return src.clone();
}
const double scale = static_cast<double>(target_h) / static_cast<double>(src.rows);
cv::Mat out;
cv::resize(src, out, cv::Size(), scale, scale, cv::INTER_LINEAR);
return out;
return std::filesystem::path(__FILE__).parent_path().parent_path();
}
/** Decode JPEG/PNG payload from sensor_msgs/CompressedImage to BGR. */
cv::Mat decodeCompressedToBgr(const uint8_t* data, size_t len)
std::string format_vector3f(const Eigen::Vector3f& value)
{
if (!data || len == 0) {
return cv::Mat();
}
cv::Mat raw(1, static_cast<int>(len), CV_8UC1, const_cast<uint8_t*>(data));
return cv::imdecode(raw, cv::IMREAD_COLOR);
std::ostringstream oss;
oss << std::fixed << std::setprecision(2)
<< "[" << value.x() << ", " << value.y() << ", " << value.z() << "]";
return oss.str();
}
cv::Mat overlayProjectedCloudOnImage(const cv::Mat& camera_bgr,
const pcl::PointCloud<pcl::PointXYZRGB>& cloud_in_cam,
const CloudReprojector::CameraParams& cam_params,
int point_radius = 2,
float min_depth = 0.5f,
float max_depth = 30.0f)
std::string format_bbox_xyxy(const std::array<float, 4>& bbox)
{
if (camera_bgr.empty()) {
return cv::Mat();
}
std::ostringstream oss;
oss << std::fixed << std::setprecision(1)
<< "[" << bbox[0] << ", " << bbox[1] << ", " << bbox[2] << ", " << bbox[3] << "]";
return oss.str();
}
cv::Mat overlay = camera_bgr.clone();
mini_vikit::PolynomialCamera camera_model(
cam_params.image_width, cam_params.image_height,
cam_params.A11, cam_params.A22,
cam_params.u0, cam_params.v0,
cam_params.A12,
cam_params.k2, cam_params.k3, cam_params.k4,
cam_params.k5, cam_params.k6, cam_params.k7);
std::vector<cv::Point> pixels;
std::vector<float> depths;
pixels.reserve(cloud_in_cam.size());
depths.reserve(cloud_in_cam.size());
for (const auto& pt : cloud_in_cam) {
if (pt.z <= min_depth || pt.z >= max_depth) {
std::string format_keypoints_xyc(
const std::array<float, 17 * 3>& keypoints_xyc,
float min_confidence = 0.5f)
{
std::ostringstream oss;
oss << std::fixed << std::setprecision(2);
bool first = true;
for (int i = 0; i < 17; ++i) {
const float confidence = keypoints_xyc[static_cast<size_t>(i) * 3 + 2];
if (confidence < min_confidence) {
continue;
}
const Eigen::Vector2d uv = camera_model.world2cam(Eigen::Vector3d(pt.x, pt.y, pt.z));
const int u = static_cast<int>(std::round(uv.x()));
const int v = static_cast<int>(std::round(uv.y()));
if (u < 0 || u >= overlay.cols || v < 0 || v >= overlay.rows) {
continue;
if (!first) {
oss << " ";
}
pixels.emplace_back(u, v);
depths.emplace_back(static_cast<float>(pt.z));
first = false;
oss << i << ":[" << keypoints_xyc[static_cast<size_t>(i) * 3 + 0]
<< ", " << keypoints_xyc[static_cast<size_t>(i) * 3 + 1]
<< ", " << confidence << "]";
}
if (pixels.empty()) {
return overlay;
if (first) {
return "none";
}
cv::Mat depth_values(static_cast<int>(depths.size()), 1, CV_32F, depths.data());
cv::Mat clipped, norm_u8, colors;
cv::min(cv::max(depth_values, min_depth), max_depth, clipped);
clipped = (clipped - min_depth) * (255.0f / (max_depth - min_depth + 1e-6f));
clipped.convertTo(norm_u8, CV_8U);
cv::applyColorMap(norm_u8, colors, cv::COLORMAP_JET);
for (int i = 0; i < colors.rows; ++i) {
const cv::Vec3b color = colors.at<cv::Vec3b>(i, 0);
cv::circle(overlay, pixels[static_cast<size_t>(i)], point_radius,
cv::Scalar(color[0], color[1], color[2]), -1);
}
return overlay;
return oss.str();
}
} // namespace
#ifdef ROS2
@@ -139,14 +107,8 @@ static bool fileExists(const std::string& filename) {
}
#ifdef ROS2
// Helper function to get package source directory for ROS2
static std::string get_package_source_directory() {
std::string current_file = __FILE__;
size_t pos = current_file.find("/src/cloud_reprojection_ros.cpp");
if (pos != std::string::npos) {
return current_file.substr(0, pos);
}
return "";
return get_package_source_directory_from_file().string();
}
// ==================== ROS2 Implementation ====================
@@ -165,7 +127,16 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op
<< "\n overlay_compressed_topic: " << sync_overlay_image_topic_
<< "\n combined_compressed_topic: " << combined_compressed_topic_
<< "\n publish_combined_compressed: " << (publish_combined_compressed_ ? "on" : "off")
<< "\n send_overlay: " << (send_overlay_ ? "on" : "off"));
<< "\n send_overlay: " << (send_overlay_ ? "on" : "off")
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
<< "\n process_target_observation: " << (enable_target_observation_ ? "on" : "off")
<< "\n debug: " << (debug_target_observation_ ? "on" : "off")
<< "\n target_track_id_topic: " << sync_target_track_id_topic_
<< "\n target_observation_topic: " << sync_target_observation_topic_
<< "\n target_pos_cam_topic: " << sync_target_pos_cam_topic_
<< "\n target_pos_world_topic: " << sync_target_pos_world_topic_
#endif
);
cloud_sub_.subscribe(this, cloud_slam_topic_);
odom_sub_.subscribe(this, odometry_topic_);
@@ -190,12 +161,38 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op
if (publish_combined_compressed_) {
combined_pub_ = this->create_publisher<CompressedImage>(combined_compressed_topic_, 10);
}
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
if (enable_target_observation_) {
target_track_id_pub_ = this->create_publisher<std_msgs::msg::Int32>(
sync_target_track_id_topic_, 10);
target_observation_pub_ = this->create_publisher<std_msgs::msg::Float32MultiArray>(
sync_target_observation_topic_, 10);
target_pos_cam_pub_ = this->create_publisher<geometry_msgs::msg::PointStamped>(
sync_target_pos_cam_topic_, 10);
target_pos_world_pub_ = this->create_publisher<geometry_msgs::msg::PointStamped>(
sync_target_pos_world_topic_, 10);
RCLCPP_INFO(
this->get_logger(),
"Target observation publishers created successfully | track_id=%s | observation=%s | pos_cam=%s | pos_world=%s",
sync_target_track_id_topic_.c_str(),
sync_target_observation_topic_.c_str(),
sync_target_pos_cam_topic_.c_str(),
sync_target_pos_world_topic_.c_str());
}
#endif
RCLCPP_INFO(this->get_logger(), "CloudReprojectionRosNode initialized (4-way sync + optional overlay + combined)");
}
void CloudReprojectionRosNode::loadParameters()
{
const auto package_path = get_package_source_directory_from_file();
const auto workspace_src_path = package_path.parent_path().parent_path().parent_path();
const auto default_yolo_engine =
(workspace_src_path / "TargetPrediction" / "deploy" / "model" / "yolo26s-pose.trt").string();
const auto default_yolo_labels =
(workspace_src_path / "TargetPrediction" / "deploy" / "YOLOs-CPP-TensorRT" / "models" / "coco.names").string();
// Declare and get parameters
this->declare_parameter<std::string>("cloud_slam_topic", "/odin1/cloud_slam");
this->declare_parameter<std::string>("odometry_topic", "/odin1/odometry");
@@ -207,6 +204,17 @@ void CloudReprojectionRosNode::loadParameters()
this->declare_parameter<int>("register_keys.send_combined_compressed", 1);
this->declare_parameter<int>("register_keys.send_overlay", 1);
this->declare_parameter<int>("register_keys.overlay_jpeg_quality", 85);
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
this->declare_parameter<int>("register_keys.process_target_observation", 1);
this->declare_parameter<int>("register_keys.debug", 1);
this->declare_parameter<std::string>("register_keys.target_yolo_engine", default_yolo_engine);
this->declare_parameter<std::string>("register_keys.target_yolo_labels", default_yolo_labels);
this->declare_parameter<double>("register_keys.target_yolo_conf", 0.45);
this->declare_parameter<double>("register_keys.target_yolo_nms", 0.50);
this->declare_parameter<double>("register_keys.target_min_depth", 0.5);
this->declare_parameter<double>("register_keys.target_max_depth", 12.0);
this->declare_parameter<double>("register_keys.target_search_radius_px", 25.0);
#endif
cloud_slam_topic_ = this->get_parameter("cloud_slam_topic").as_string();
odometry_topic_ = this->get_parameter("odometry_topic").as_string();
@@ -228,10 +236,13 @@ void CloudReprojectionRosNode::loadParameters()
sync_wiwc_topic_ = sync_topic_prefix_ + "/wiwc";
sync_image_topic_ = sync_topic_prefix_ + "/image";
sync_overlay_image_topic_ = sync_topic_prefix_ + "/overlay_img/compressed";
sync_target_track_id_topic_ = sync_topic_prefix_ + "/target_track_id";
sync_target_observation_topic_ = sync_topic_prefix_ + "/target_observation";
sync_target_pos_cam_topic_ = sync_topic_prefix_ + "/target_pos_cam";
sync_target_pos_world_topic_ = sync_topic_prefix_ + "/target_pos_world";
// Load camera parameters from calib.yaml file directly
std::string package_path = get_package_source_directory();
std::string calib_file = package_path + "/config/calib.yaml";
std::string calib_file = (package_path / "config" / "calib.yaml").string();
YAML::Node calib_config;
try {
@@ -304,6 +315,63 @@ void CloudReprojectionRosNode::loadParameters()
RCLCPP_ERROR(this->get_logger(), "Failed to initialize CloudReprojector");
rclcpp::shutdown();
}
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
enable_target_observation_ =
(this->get_parameter("register_keys.process_target_observation").as_int() != 0);
debug_target_observation_ =
(this->get_parameter("register_keys.debug").as_int() != 0);
if (enable_target_observation_) {
try {
odin_ros_driver::TargetObservationConfig target_config;
target_config.yolo_engine_path =
this->get_parameter("register_keys.target_yolo_engine").as_string();
target_config.yolo_labels_path =
this->get_parameter("register_keys.target_yolo_labels").as_string();
target_config.yolo_conf = static_cast<float>(
this->get_parameter("register_keys.target_yolo_conf").as_double());
target_config.yolo_nms = static_cast<float>(
this->get_parameter("register_keys.target_yolo_nms").as_double());
target_config.min_depth = static_cast<float>(
this->get_parameter("register_keys.target_min_depth").as_double());
target_config.max_depth = static_cast<float>(
this->get_parameter("register_keys.target_max_depth").as_double());
target_config.search_radius_px = static_cast<float>(
this->get_parameter("register_keys.target_search_radius_px").as_double());
target_config.debug = debug_target_observation_;
target_observation_processor_ =
std::make_unique<odin_ros_driver::TargetObservationProcessor>();
target_observation_processor_->initialize(target_config);
RCLCPP_INFO(
this->get_logger(),
"Target observation enabled | engine=%s",
target_config.yolo_engine_path.c_str());
RCLCPP_INFO(
this->get_logger(),
"Target observation processor initialized successfully | labels=%s",
target_config.yolo_labels_path.empty() ? "<default-person>" : target_config.yolo_labels_path.c_str());
if (debug_target_observation_) {
RCLCPP_INFO(
this->get_logger(),
"Target observation debug enabled | yolo_engine=%s | yolo_labels=%s | yolo_conf=%.3f | yolo_nms=%.3f | min_depth=%.2f | max_depth=%.2f | search_radius_px=%.1f",
target_config.yolo_engine_path.c_str(),
target_config.yolo_labels_path.empty() ? "<default-person>" : target_config.yolo_labels_path.c_str(),
target_config.yolo_conf,
target_config.yolo_nms,
target_config.min_depth,
target_config.max_depth,
target_config.search_radius_px);
}
} catch (const std::exception& e) {
enable_target_observation_ = false;
target_observation_processor_.reset();
RCLCPP_ERROR(
this->get_logger(),
"Failed to initialize target observation processor: %s",
e.what());
}
}
#endif
}
void CloudReprojectionRosNode::syncCallback(
@@ -388,7 +456,11 @@ void CloudReprojectionRosNode::syncCallback(
}
cv::Mat cam_bgr;
if (send_overlay_ || need_combined) {
if (send_overlay_ || need_combined
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
|| enable_target_observation_
#endif
) {
try {
cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(image_msg, "bgr8");
cam_bgr = cv_ptr->image;
@@ -404,9 +476,157 @@ void CloudReprojectionRosNode::syncCallback(
sync_cloud_pub_->publish(cloud_cam_msg);
sync_image_pub_->publish(sync_image_msg);
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
odin_ros_driver::TargetObservation target_observation;
odin_ros_driver::TargetObservationDebugInfo target_debug;
if (enable_target_observation_ && target_observation_processor_) {
const auto target_start = std::chrono::steady_clock::now();
target_observation = target_observation_processor_->process(
cam_bgr,
cloud_cam,
*reprojector_,
odom_pose,
debug_target_observation_ ? &target_debug : nullptr);
const auto target_end = std::chrono::steady_clock::now();
if (debug_target_observation_) {
if (!target_observation.valid) {
if (target_debug.poses_count == 0) {
RCLCPP_INFO_THROTTLE(
this->get_logger(),
*this->get_clock(),
2000,
"Target observation | no detections | yolo=%.2f ms | total=%.2f ms",
target_debug.yolo_ms,
target_debug.total_ms);
} else if (target_debug.tracks_count == 0) {
RCLCPP_INFO_THROTTLE(
this->get_logger(),
*this->get_clock(),
2000,
"Target observation | detections=%d tracked=0 current_target_id=%d | yolo=%.2f ms | mot=%.2f ms | total=%.2f ms",
target_debug.poses_count,
target_debug.current_target_id_before,
target_debug.yolo_ms,
target_debug.mot_ms,
target_debug.total_ms);
} else {
RCLCPP_INFO_THROTTLE(
this->get_logger(),
*this->get_clock(),
1000,
"Target observation | detections=%d tracked=%d selected_id=%d det_ind=%d cloud_pts=%d depth_samples=%d | yolo=%.2f ms | mot=%.2f ms | depth=%.2f ms | total=%.2f ms",
target_debug.poses_count,
target_debug.tracks_count,
target_debug.selected_track_id,
target_debug.detection_index,
target_debug.projected_cloud_points,
target_debug.depth_sample_count,
target_debug.yolo_ms,
target_debug.mot_ms,
target_debug.depth_ms,
target_debug.total_ms);
}
} else {
RCLCPP_INFO_THROTTLE(
this->get_logger(),
*this->get_clock(),
500,
"Target observation | stamp=%u.%u size=%dx%d cloud=%zu detections=%d tracked=%d current_target_id=%d selected_id=%d det_ind=%d reused=%s bbox=%s depth=%.3f conf=%.3f depth_conf=%.3f pos_cam=%s pos_world=%s cloud_pts=%d depth_samples=%d | yolo=%.2f ms | mot=%.2f ms | depth=%.2f ms | total=%.2f ms | node_total=%.2f ms",
image_msg->header.stamp.sec,
image_msg->header.stamp.nanosec,
cam_bgr.cols,
cam_bgr.rows,
cloud_cam.size(),
target_debug.poses_count,
target_debug.tracks_count,
target_debug.current_target_id_before,
target_observation.track_id,
target_observation.detection_index,
target_debug.found_existing_target ? "yes" : "no",
format_bbox_xyxy(target_observation.bbox_xyxy).c_str(),
target_observation.depth,
target_observation.confidence,
target_observation.depth_confidence,
format_vector3f(target_observation.target_pos_cam).c_str(),
format_vector3f(target_observation.target_pos_world).c_str(),
target_debug.projected_cloud_points,
target_debug.depth_sample_count,
target_debug.yolo_ms,
target_debug.mot_ms,
target_debug.depth_ms,
target_debug.total_ms,
std::chrono::duration<double, std::milli>(target_end - target_start).count());
RCLCPP_INFO_THROTTLE(
this->get_logger(),
*this->get_clock(),
500,
"Target observation keypoints: %s",
format_keypoints_xyc(target_observation.keypoints_xyc).c_str());
}
}
if (target_observation.valid) {
std_msgs::msg::Int32 track_id_msg;
track_id_msg.data = target_observation.track_id;
target_track_id_pub_->publish(track_id_msg);
std_msgs::msg::Float32MultiArray observation_msg;
observation_msg.data.reserve(4 + 17 * 3 + 3 + 3 + 3);
observation_msg.data.insert(
observation_msg.data.end(),
target_observation.bbox_xyxy.begin(),
target_observation.bbox_xyxy.end());
observation_msg.data.insert(
observation_msg.data.end(),
target_observation.keypoints_xyc.begin(),
target_observation.keypoints_xyc.end());
observation_msg.data.push_back(target_observation.target_pos_cam.x());
observation_msg.data.push_back(target_observation.target_pos_cam.y());
observation_msg.data.push_back(target_observation.target_pos_cam.z());
observation_msg.data.push_back(target_observation.target_pos_world.x());
observation_msg.data.push_back(target_observation.target_pos_world.y());
observation_msg.data.push_back(target_observation.target_pos_world.z());
observation_msg.data.push_back(target_observation.confidence);
observation_msg.data.push_back(target_observation.depth);
observation_msg.data.push_back(target_observation.depth_confidence);
target_observation_pub_->publish(observation_msg);
geometry_msgs::msg::PointStamped pos_cam_msg;
pos_cam_msg.header = image_msg->header;
pos_cam_msg.header.stamp = sync_stamp;
pos_cam_msg.header.frame_id = cloud_cam_msg.header.frame_id.empty()
? "camera"
: cloud_cam_msg.header.frame_id;
pos_cam_msg.point.x = target_observation.target_pos_cam.x();
pos_cam_msg.point.y = target_observation.target_pos_cam.y();
pos_cam_msg.point.z = target_observation.target_pos_cam.z();
target_pos_cam_pub_->publish(pos_cam_msg);
geometry_msgs::msg::PointStamped pos_world_msg;
pos_world_msg.header = image_msg->header;
pos_world_msg.header.stamp = sync_stamp;
pos_world_msg.header.frame_id = odom_msg->header.frame_id.empty()
? "odom"
: odom_msg->header.frame_id;
pos_world_msg.point.x = target_observation.target_pos_world.x();
pos_world_msg.point.y = target_observation.target_pos_world.y();
pos_world_msg.point.z = target_observation.target_pos_world.z();
target_pos_world_pub_->publish(pos_world_msg);
}
}
#endif
if (send_overlay_ && overlay_compressed_pub_) {
cv::Mat overlay_vis = overlayProjectedCloudOnImage(
cv::Mat overlay_vis = odin_ros_driver::overlay_projected_cloud_on_image(
cam_bgr, cloud_cam, reprojector_->getCameraParams());
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
if (target_observation.valid && debug_target_observation_) {
odin_ros_driver::draw_target_observation_overlay(
overlay_vis,
target_observation);
}
#endif
if (!overlay_vis.empty()) {
std::vector<uchar> obuf;
const std::vector<int> oenc = {cv::IMWRITE_JPEG_QUALITY, overlay_jpeg_quality_};
@@ -424,8 +644,8 @@ void CloudReprojectionRosNode::syncCallback(
if (combined_pub_) {
const int H = std::max(depth_vis.rows, cam_bgr.rows);
cv::Mat left = resizeToHeight(depth_vis, H);
cv::Mat right = resizeToHeight(cam_bgr, H);
cv::Mat left = odin_ros_driver::resize_to_height(depth_vis, H);
cv::Mat right = odin_ros_driver::resize_to_height(cam_bgr, H);
cv::Mat combined;
cv::hconcat(left, right, combined);
@@ -720,7 +940,7 @@ void CloudReprojectionRosNode::syncCallback(
if (send_overlay_) {
cv::Mat overlay_vis =
overlayProjectedCloudOnImage(cam_bgr, cloud_cam, reprojector_->getCameraParams());
odin_ros_driver::overlay_projected_cloud_on_image(cam_bgr, cloud_cam, reprojector_->getCameraParams());
if (!overlay_vis.empty()) {
std::vector<uchar> obuf;
const std::vector<int> oenc = {cv::IMWRITE_JPEG_QUALITY, overlay_jpeg_quality_};
@@ -738,8 +958,8 @@ void CloudReprojectionRosNode::syncCallback(
if (publish_combined_compressed_) {
const int H = std::max(depth_vis.rows, cam_bgr.rows);
cv::Mat left = resizeToHeight(depth_vis, H);
cv::Mat right = resizeToHeight(cam_bgr, H);
cv::Mat left = odin_ros_driver::resize_to_height(depth_vis, H);
cv::Mat right = odin_ros_driver::resize_to_height(cam_bgr, H);
cv::Mat combined;
cv::hconcat(left, right, combined);