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

add multi-target support (TargetObservation + rviz publish);

Browse Source 
现在可以发布包含多个目标的消息了，而且自带多目标管理系统，可以支持发布多目标的info，debug和后续推理都可以
This commit is contained in:
hjy
2026-04-19 16:48:54 +08:00
parent a1fddc9638
commit 5b177c90f4
11 changed files with 910 additions and 461 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+2
View File
@@ -356,6 +356,7 @@ elseif(ROS_VERSION STREQUAL "ROS2")
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/TargetObservation.msg"
"msg/TargetObservationArray.msg"
DEPENDENCIES std_msgs geometry_msgs nav_msgs
)
@@ -466,6 +467,7 @@ elseif(ROS_VERSION STREQUAL "ROS2")
nav_msgs
geometry_msgs
std_msgs
visualization_msgs
cv_bridge
pcl_conversions
message_filters
config/odin_ros2.rviz
+16 -21
View File
@@ -13,7 +13,8 @@ Panels:
- /slam1
- /dense_depth_demo1
- /Prediction1
- /Prediction1/Path1
- /Prediction1/detection_pos_world1
- /Prediction1/detection_pos_cam1
- /Prediction1/Marker1
- /Planning1
- /Planning1/GridMap1
@@ -424,35 +425,30 @@ Visualization Manager:
Reliability Policy: Reliable
Value: /target/pred_image
Value: false
- Alpha: 1
Class: rviz_default_plugins/PointStamped
Color: 224; 27; 36
Enabled: false
History Length: 10
Name: target_pos_world
Radius: 0.30000001192092896
- Class: rviz_default_plugins/MarkerArray
Enabled: true
Name: detection_pos_world
Namespaces:
detection_label: true
detection_sphere: true
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /odin1/sync/target_pos_world
Value: false
- Alpha: 1
Class: rviz_default_plugins/PointStamped
Color: 204; 41; 204
Value: /odin1/sync/detection_pos_world
Value: true
- Class: rviz_default_plugins/MarkerArray
Enabled: false
History Length: 1
Name: target_pos_cam
Radius: 0.20000000298023224
Name: detection_pos_cam
Namespaces:
{}
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /odin1/sync/target_pos_cam
Value: /odin1/sync/detection_pos_cam
Value: false
- Alpha: 1
Buffer Length: 1
@@ -542,8 +538,7 @@ Visualization Manager:
Enabled: true
Name: path
Namespaces:
future_path: true
future_yaw: true
{}
Topic:
Depth: 5
Durability Policy: Volatile
include/cloud_reprojection_ros_node.hpp
+9 -4
View File
@@ -22,10 +22,13 @@ limitations under the License.
#include <nav_msgs/msg/odometry.hpp>
#include <std_msgs/msg/float32_multi_array.hpp>
#include <std_msgs/msg/int32.hpp>
#include <visualization_msgs/msg/marker.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include "odin_ros_driver/msg/target_observation.hpp"
#include "odin_ros_driver/msg/target_observation_array.hpp"
#else
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
@@ -96,8 +99,9 @@ private:
std::string sync_overlay_image_topic_;
std::string sync_detection_debug_image_topic_;
std::string sync_target_observation_topic_;
std::string sync_target_pos_cam_topic_;
std::string sync_target_pos_world_topic_;
std::string sync_track_observations_topic_;
std::string sync_detection_pos_cam_topic_;
std::string sync_detection_pos_world_topic_;
rclcpp::Publisher<PointCloud2>::SharedPtr sync_cloud_pub_;
rclcpp::Publisher<PointCloud2>::SharedPtr sync_cloud_slam_pub_;
@@ -110,8 +114,9 @@ private:
rclcpp::Publisher<CompressedImage>::SharedPtr detection_debug_compressed_pub_; // debug detections/tracks
rclcpp::Publisher<CompressedImage>::SharedPtr combined_pub_; // optional if publish_combined_compressed_
rclcpp::Publisher<odin_ros_driver::msg::TargetObservation>::SharedPtr target_observation_pub_;
rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr target_pos_cam_pub_;
rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr target_pos_world_pub_;
rclcpp::Publisher<odin_ros_driver::msg::TargetObservationArray>::SharedPtr track_observations_pub_;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr detection_pos_cam_pub_;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr detection_pos_world_pub_;
std::unique_ptr<CloudReprojector> reprojector_;
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
include/target_observation_processing.hpp
+100 -25
View File
@@ -4,6 +4,7 @@
#include <cstdint>
#include <memory>
#include <string>
#include <unordered_map>
#include <vector>
#include <Eigen/Dense>
@@ -42,6 +43,13 @@ struct TargetObservationConfig {
int reid_input_width = 128;
int reid_feature_dim = 512;
int reid_max_batch_size = 8;
int max_concurrent_tracks = 16;
// 3D position gate for ReID rebind. dmax = base_m + per_frame_m *
// frames_since_last_seen. soft mode (hard=false) logs via debug counters
// but still allows the similarity match; hard mode rejects the pair.
float reid_3d_gate_base_m = 1.5f;
float reid_3d_gate_per_frame_m = 0.1f;
bool reid_3d_gate_hard = false;
bool debug = false;
};
@@ -50,13 +58,14 @@ struct TargetObservation {
int track_id = -1;
int raw_track_id = -1;
int detection_index = -1;
bool is_primary_target = false;
float confidence = 0.0f;
float depth = -1.0f;
float depth_confidence = 0.0f;
std::array<float, 4> bbox_xyxy{0.0f, 0.0f, 0.0f, 0.0f};
std::array<float, 17 * 3> keypoints_xyc{};
Eigen::Vector3f target_pos_cam = Eigen::Vector3f::Zero();
Eigen::Vector3f target_pos_world = Eigen::Vector3f::Zero();
Eigen::Vector3f detection_pos_cam = Eigen::Vector3f::Zero();
Eigen::Vector3f detection_pos_world = Eigen::Vector3f::Zero();
};
struct TargetObservationDebugInfo {
@@ -64,6 +73,7 @@ struct TargetObservationDebugInfo {
int current_raw_track_id_before = -1;
int poses_count = 0;
int tracks_count = 0;
int active_tracks_count = 0;
int selected_track_id = -1;
int selected_raw_track_id = -1;
int detection_index = -1;
@@ -71,6 +81,8 @@ struct TargetObservationDebugInfo {
int depth_sample_count = 0;
int gallery_size = 0;
int lost_frames = 0;
int reid_3d_gate_rejections = 0; // hard-mode rejections this frame
int reid_3d_gate_soft_warnings = 0; // soft-mode over-threshold this frame
bool found_existing_target = false;
bool target_selected = false;
bool selected_from_center_bootstrap = false;
@@ -81,7 +93,13 @@ struct TargetObservationDebugInfo {
float reid_similarity = -1.0f;
float yolo_ms = 0.0f;
float mot_ms = 0.0f;
float depth_ms = 0.0f;
float cloud_project_ms = 0.0f;
float bind_ms = 0.0f; // entire bind_and_update_tracks (covers depth_ms)
float depth_ms = 0.0f; // Phase 2A per-entry 3D compute (subset of bind_ms)
float reid_extract_ms = 0.0f; // Phase 2B TRT feature extraction (subset of bind_ms)
float age_out_ms = 0.0f;
float select_ms = 0.0f;
float enrich_ms = 0.0f;
float total_ms = 0.0f;
std::vector<yolos::pose::PoseResult> poses;
std::vector<cv::Point> valid_projected_pixels;
@@ -100,6 +118,12 @@ public:
const CloudReprojector::OdomPose& odom_pose,
TargetObservationDebugInfo* debug_info = nullptr);
// All active tracks with a fresh 3D estimate from the most recent
// process() call. One TargetObservation per entry; is_primary_target
// marks the follow target. Empty if process() hasn't run yet.
std::vector<TargetObservation> snapshot_active_tracks() const;
int current_target_stable_id() const { return current_target_stable_id_; }
bool initialized() const { return initialized_; }
void draw_detected_poses(
@@ -107,55 +131,106 @@ public:
const std::vector<yolos::pose::PoseResult>& poses) const;
private:
struct ProjectedCloud {
std::vector<cv::Point2f> uv;
std::vector<Eigen::Vector3f> cam;
};
struct EntryCloudEstimate {
bool valid = false;
float depth = -1.0f;
float depth_confidence = 0.0f;
Eigen::Vector3f pos_cam = Eigen::Vector3f::Zero();
Eigen::Vector3f pos_world = Eigen::Vector3f::Zero();
int depth_sample_count = 0;
std::vector<cv::Point> valid_projected_pixels;
};
struct TrackEntry {
int stable_id = -1;
int last_raw_track_id = -1;
TrackGallery gallery;
cv::Rect last_bbox;
bool has_last_bbox = false;
int64_t last_seen_frame = -1;
bool just_rebound_by_reid = false;
float last_rebind_similarity = -1.0f;
// Per-entry 3D state; refreshed whenever compute_entry_3d succeeds.
float last_depth = -1.0f;
float last_depth_confidence = 0.0f;
Eigen::Vector3f last_pos_cam = Eigen::Vector3f::Zero();
Eigen::Vector3f last_pos_world = Eigen::Vector3f::Zero();
bool has_last_pos = false;
int64_t last_pos_frame = -1;
// 2D observation cache — refreshed every frame in Phase 5 while the
// entry is seen. Consumed by snapshot_active_tracks() to populate
// TargetObservation messages for all tracked people.
std::array<float, 17 * 3> last_keypoints_xyc{};
int last_detection_index = -1;
float last_confidence = 0.0f;
// Debug snapshot from the most recent successful estimate — only the
// follow target's values are copied into TargetObservationDebugInfo.
int last_depth_sample_count = 0;
std::vector<cv::Point> last_valid_projected_pixels;
};
Eigen::MatrixXf format_detections(
const std::vector<yolos::pose::PoseResult>& poses) const;
std::vector<yolos::pose::PoseResult> detect_poses_with_lost_compensation(
const cv::Mat& camera_bgr,
TargetObservationDebugInfo* debug_info) const;
cv::Rect build_lost_detection_roi(const cv::Size& image_size) const;
cv::Rect build_lost_detection_roi(
const cv::Size& image_size,
const cv::Rect& last_bbox) const;
static void offset_poses(
std::vector<yolos::pose::PoseResult>& poses,
const cv::Point& offset);
TargetObservation select_target(
ProjectedCloud project_cloud_to_image(
const pcl::PointCloud<pcl::PointXYZRGB>& cloud_in_cam,
const CloudReprojector& reprojector) const;
EntryCloudEstimate compute_entry_3d(
const yolos::pose::PoseResult& pose,
const std::array<float, 4>& bbox_xyxy,
const ProjectedCloud& projected,
const CloudReprojector& reprojector,
const CloudReprojector::OdomPose& odom_pose) const;
std::vector<int> bind_and_update_tracks(
const cv::Mat& camera_bgr,
const Eigen::MatrixXf& tracks,
const std::vector<yolos::pose::PoseResult>& poses,
const ProjectedCloud& projected,
const CloudReprojector& reprojector,
const CloudReprojector::OdomPose& odom_pose,
TargetObservationDebugInfo* debug_info);
void age_out_tracks();
TargetObservation select_follow_target(
const std::vector<yolos::pose::PoseResult>& poses,
const Eigen::MatrixXf& tracks,
const std::vector<int>& stable_ids,
int image_width,
int image_height,
TargetObservationDebugInfo* debug_info);
TargetObservation enrich_target_with_cloud(
const TargetObservation& target,
const std::vector<yolos::pose::PoseResult>& poses,
const pcl::PointCloud<pcl::PointXYZRGB>& cloud_in_cam,
const CloudReprojector& reprojector,
const CloudReprojector::OdomPose& odom_pose,
TargetObservationDebugInfo* debug_info) const;
int recover_target_with_reid(
const cv::Mat& camera_bgr,
const std::vector<yolos::pose::PoseResult>& poses,
float* best_similarity) const;
void update_target_gallery(
const cv::Mat& camera_bgr,
const TargetObservation& target,
bool force_update);
bool should_update_gallery(bool force_update) const;
bool is_good_reid_crop(const cv::Rect& bbox, const cv::Size& image_size) const;
TargetObservationConfig config_;
std::unique_ptr<yolos::pose::YOLOPoseDetector> yolo_;
std::unique_ptr<motcpp::trackers::ByteTrack> tracker_;
std::unique_ptr<ReIDTensorRTExtractor> reid_extractor_;
TrackGallery target_gallery_;
int current_target_id_ = -1;
int current_raw_track_id_ = -1;
cv::Rect last_target_bbox_;
bool has_last_target_bbox_ = false;
std::unordered_map<int, TrackEntry> tracks_;
int next_stable_id_ = 0;
int current_target_stable_id_ = -1;
int64_t frame_index_ = 0;
int lost_track_frames_ = 0;
bool initialized_ = false;
};
msg/TargetObservation.msg
+3 -2
View File
@@ -3,10 +3,11 @@ nav_msgs/Odometry odometry
bool valid
int32 track_id
int32 detection_index
bool is_primary_target
float32 confidence
float32 depth
float32 depth_confidence
float32[4] bbox_xyxy
float32[51] keypoints_xyc
geometry_msgs/Point target_pos_cam
geometry_msgs/Point target_pos_world
geometry_msgs/Point detection_pos_cam
geometry_msgs/Point detection_pos_world
msg/TargetObservationArray.msg
+3
View File
@@ -0,0 +1,3 @@
std_msgs/Header header
int32 primary_target_id
TargetObservation[] observations
package.xml
+1
View File
@@ -15,6 +15,7 @@
<depend>sensor_msgs</depend>
<depend>nav_msgs</depend>
<depend>geometry_msgs</depend>
<depend>visualization_msgs</depend>
<depend>cv_bridge</depend>
<depend>image_transport</depend>
<depend>pcl_conversions</depend>
package_ros2.xml
+1
View File
@@ -15,6 +15,7 @@
<depend>sensor_msgs</depend>
<depend>nav_msgs</depend>
<depend>geometry_msgs</depend>
<depend>visualization_msgs</depend>
<depend>cv_bridge</depend>
<depend>image_transport</depend>
<depend>pcl_conversions</depend>
script/record_vis.sh
+3 -2
View File
@@ -4,7 +4,8 @@ ros2 bag record \
/odin1/odometry \
/odin1/wiwc \
/tf \
/odin1/sync/target_pos_world \
/odin1/sync/target_pos_cam \
/odin1/sync/detection_pos_world \
/odin1/sync/detection_pos_cam \
/odin1/sync/target_observation \
/odin1/sync/track_observations \
src/cloud_reprojection_ros.cpp
+175 -73
View File
@@ -160,8 +160,9 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op
<< "\n process_target_observation: " << (enable_target_observation_ ? "on" : "off")
<< "\n debug: " << (debug_target_observation_ ? "on" : "off")
<< "\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_
<< "\n track_observations_topic: " << sync_track_observations_topic_
<< "\n detection_pos_cam_topic: " << sync_detection_pos_cam_topic_
<< "\n detection_pos_world_topic: " << sync_detection_pos_world_topic_
#endif
);
@@ -217,16 +218,23 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op
if (enable_target_observation_) {
target_observation_pub_ = this->create_publisher<odin_ros_driver::msg::TargetObservation>(
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);
track_observations_pub_ =
this->create_publisher<odin_ros_driver::msg::TargetObservationArray>(
sync_track_observations_topic_, 10);
detection_pos_cam_pub_ =
this->create_publisher<visualization_msgs::msg::MarkerArray>(
sync_detection_pos_cam_topic_, 10);
detection_pos_world_pub_ =
this->create_publisher<visualization_msgs::msg::MarkerArray>(
sync_detection_pos_world_topic_, 10);
RCLCPP_INFO(
this->get_logger(),
"Target observation publishers created successfully | observation=%s | pos_cam=%s | pos_world=%s",
"Target observation publishers created | observation=%s | tracks=%s | "
"detection_pos_cam=%s | detection_pos_world=%s",
sync_target_observation_topic_.c_str(),
sync_target_pos_cam_topic_.c_str(),
sync_target_pos_world_topic_.c_str());
sync_track_observations_topic_.c_str(),
sync_detection_pos_cam_topic_.c_str(),
sync_detection_pos_world_topic_.c_str());
}
#endif
@@ -311,8 +319,9 @@ void CloudReprojectionRosNode::loadParameters()
sync_overlay_image_topic_ = sync_topic_prefix_ + "/overlay_img/compressed";
sync_detection_debug_image_topic_ = sync_topic_prefix_ + "/detection_img_debug/compressed";
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";
sync_track_observations_topic_ = sync_topic_prefix_ + "/track_observations";
sync_detection_pos_cam_topic_ = sync_topic_prefix_ + "/detection_pos_cam";
sync_detection_pos_world_topic_ = sync_topic_prefix_ + "/detection_pos_world";
// Load camera parameters from calib.yaml file directly
std::string calib_file = (package_path / "config" / "calib.yaml").string();
@@ -662,9 +671,10 @@ void CloudReprojectionRosNode::processSyncedData(
this->get_logger(),
*this->get_clock(),
1000,
"Target observation | detections=%d tracked=%d selected_id=%d raw_id=%d det_ind=%d center_fallback=%s lost_comp_attempted=%s lost_comp_recovered=%s reid_attempted=%s reid_recovered=%s reid_sim=%.3f gallery=%d lost=%d cloud_pts=%d depth_samples=%d | yolo=%.2f ms | mot=%.2f ms | depth=%.2f ms | total=%.2f ms",
"Target observation | detections=%d tracked=%d active=%d selected_id=%d raw_id=%d det_ind=%d center_fallback=%s lost_comp_attempted=%s lost_comp_recovered=%s reid_attempted=%s reid_recovered=%s reid_sim=%.3f gallery=%d lost=%d cloud_pts=%d depth_samples=%d | yolo=%.2f mot=%.2f cloud=%.2f bind=%.2f (depth=%.2f reid=%.2f) age=%.2f select=%.2f enrich=%.2f | total=%.2f ms",
target_debug.poses_count,
target_debug.tracks_count,
target_debug.active_tracks_count,
target_debug.selected_track_id,
target_debug.selected_raw_track_id,
target_debug.detection_index,
@@ -680,7 +690,13 @@ void CloudReprojectionRosNode::processSyncedData(
target_debug.depth_sample_count,
target_debug.yolo_ms,
target_debug.mot_ms,
target_debug.cloud_project_ms,
target_debug.bind_ms,
target_debug.depth_ms,
target_debug.reid_extract_ms,
target_debug.age_out_ms,
target_debug.select_ms,
target_debug.enrich_ms,
target_debug.total_ms);
}
} else {
@@ -688,7 +704,7 @@ void CloudReprojectionRosNode::processSyncedData(
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 current_raw_id=%d selected_id=%d selected_raw_id=%d det_ind=%d reused=%s center_fallback=%s lost_comp_attempted=%s lost_comp_recovered=%s reid_attempted=%s reid_recovered=%s reid_sim=%.3f gallery=%d lost=%d 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",
"Target observation | stamp=%u.%u size=%dx%d cloud=%zu detections=%d tracked=%d active=%d current_target_id=%d current_raw_id=%d selected_id=%d selected_raw_id=%d det_ind=%d reused=%s center_fallback=%s lost_comp_attempted=%s lost_comp_recovered=%s reid_attempted=%s reid_recovered=%s reid_sim=%.3f gate_rej=%d gate_soft=%d gallery=%d lost=%d bbox=%s depth=%.3f conf=%.3f depth_conf=%.3f pos_cam=%s pos_world=%s cloud_pts=%d depth_samples=%d | yolo=%.2f mot=%.2f cloud=%.2f bind=%.2f (depth=%.2f reid=%.2f) age=%.2f select=%.2f enrich=%.2f | total=%.2f ms | node_total=%.2f ms",
image_msg.header.stamp.sec,
image_msg.header.stamp.nanosec,
cam_bgr.cols,
@@ -696,6 +712,7 @@ void CloudReprojectionRosNode::processSyncedData(
cloud_cam.size(),
target_debug.poses_count,
target_debug.tracks_count,
target_debug.active_tracks_count,
target_debug.current_target_id_before,
target_debug.current_raw_track_id_before,
target_observation.track_id,
@@ -708,19 +725,27 @@ void CloudReprojectionRosNode::processSyncedData(
target_debug.reid_attempted ? "yes" : "no",
target_debug.recovered_by_reid ? "yes" : "no",
target_debug.reid_similarity,
target_debug.reid_3d_gate_rejections,
target_debug.reid_3d_gate_soft_warnings,
target_debug.gallery_size,
target_debug.lost_frames,
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(),
format_vector3f(target_observation.detection_pos_cam).c_str(),
format_vector3f(target_observation.detection_pos_world).c_str(),
target_debug.projected_cloud_points,
target_debug.depth_sample_count,
target_debug.yolo_ms,
target_debug.mot_ms,
target_debug.cloud_project_ms,
target_debug.bind_ms,
target_debug.depth_ms,
target_debug.reid_extract_ms,
target_debug.age_out_ms,
target_debug.select_ms,
target_debug.enrich_ms,
target_debug.total_ms,
std::chrono::duration<double, std::milli>(target_end - target_start).count());
RCLCPP_INFO_THROTTLE(
@@ -732,75 +757,152 @@ void CloudReprojectionRosNode::processSyncedData(
}
}
// Helper: fill a TargetObservation ROS msg from the C++ struct.
auto fill_observation_msg = [](
odin_ros_driver::msg::TargetObservation& msg,
const odin_ros_driver::TargetObservation& obs) {
msg.valid = obs.valid;
if (obs.valid) {
msg.track_id = obs.track_id;
msg.detection_index = obs.detection_index;
msg.is_primary_target = obs.is_primary_target;
msg.confidence = obs.confidence;
msg.depth = obs.depth;
msg.depth_confidence = obs.depth_confidence;
msg.bbox_xyxy = obs.bbox_xyxy;
msg.keypoints_xyc = obs.keypoints_xyc;
msg.detection_pos_cam.x = obs.detection_pos_cam.x();
msg.detection_pos_cam.y = obs.detection_pos_cam.y();
msg.detection_pos_cam.z = obs.detection_pos_cam.z();
msg.detection_pos_world.x = obs.detection_pos_world.x();
msg.detection_pos_world.y = obs.detection_pos_world.y();
msg.detection_pos_world.z = obs.detection_pos_world.z();
} else {
msg.track_id = -1;
msg.detection_index = -1;
msg.is_primary_target = false;
msg.confidence = -1.0f;
msg.depth = -1.0f;
msg.depth_confidence = -1.0f;
msg.bbox_xyxy.fill(-1.0f);
msg.keypoints_xyc.fill(-1.0f);
msg.detection_pos_cam.x = -1.0;
msg.detection_pos_cam.y = -1.0;
msg.detection_pos_cam.z = -1.0;
msg.detection_pos_world.x = -1.0;
msg.detection_pos_world.y = -1.0;
msg.detection_pos_world.z = -1.0;
}
};
// Single-target topic — still published for backwards-compatible
// downstream that only cares about the follow target.
odin_ros_driver::msg::TargetObservation observation_msg;
observation_msg.header = image_msg.header;
observation_msg.header.stamp = sync_stamp;
observation_msg.odometry = sync_odom_msg;
observation_msg.valid = target_observation.valid;
if (target_observation.valid) {
observation_msg.track_id = target_observation.track_id;
observation_msg.detection_index = target_observation.detection_index;
observation_msg.confidence = target_observation.confidence;
observation_msg.depth = target_observation.depth;
observation_msg.depth_confidence = target_observation.depth_confidence;
observation_msg.bbox_xyxy = target_observation.bbox_xyxy;
observation_msg.keypoints_xyc = target_observation.keypoints_xyc;
observation_msg.target_pos_cam.x = target_observation.target_pos_cam.x();
observation_msg.target_pos_cam.y = target_observation.target_pos_cam.y();
observation_msg.target_pos_cam.z = target_observation.target_pos_cam.z();
observation_msg.target_pos_world.x = target_observation.target_pos_world.x();
observation_msg.target_pos_world.y = target_observation.target_pos_world.y();
observation_msg.target_pos_world.z = target_observation.target_pos_world.z();
} else {
observation_msg.track_id = -1;
observation_msg.detection_index = -1;
observation_msg.confidence = -1.0f;
observation_msg.depth = -1.0f;
observation_msg.depth_confidence = -1.0f;
observation_msg.bbox_xyxy.fill(-1.0f);
observation_msg.keypoints_xyc.fill(-1.0f);
observation_msg.target_pos_cam.x = -1.0;
observation_msg.target_pos_cam.y = -1.0;
observation_msg.target_pos_cam.z = -1.0;
observation_msg.target_pos_world.x = -1.0;
observation_msg.target_pos_world.y = -1.0;
observation_msg.target_pos_world.z = -1.0;
}
fill_observation_msg(observation_msg, target_observation);
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;
if (target_observation.valid) {
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();
} else {
pos_cam_msg.point.x = -1.0;
pos_cam_msg.point.y = -1.0;
pos_cam_msg.point.z = -1.0;
// Multi-target topic — every active track with a fresh 3D estimate,
// including the follow target (marked via is_primary_target).
const std::vector<odin_ros_driver::TargetObservation> active_tracks =
target_observation_processor_->snapshot_active_tracks();
odin_ros_driver::msg::TargetObservationArray track_array_msg;
track_array_msg.header = image_msg.header;
track_array_msg.header.stamp = sync_stamp;
track_array_msg.primary_target_id =
target_observation_processor_->current_target_stable_id();
track_array_msg.observations.reserve(active_tracks.size());
for (const auto& obs : active_tracks) {
odin_ros_driver::msg::TargetObservation one;
one.header = image_msg.header;
one.header.stamp = sync_stamp;
one.odometry = sync_odom_msg;
fill_observation_msg(one, obs);
track_array_msg.observations.push_back(std::move(one));
}
target_pos_cam_pub_->publish(pos_cam_msg);
track_observations_pub_->publish(track_array_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"
// Debug Marker topics — rendered directly in RViz. Primary target in
// red, others in cyan; short lifetime so stale markers auto-clear.
std_msgs::msg::Header cam_header = image_msg.header;
cam_header.stamp = sync_stamp;
cam_header.frame_id = cloud_cam_msg.header.frame_id.empty()
? std::string("camera")
: cloud_cam_msg.header.frame_id;
std_msgs::msg::Header world_header = image_msg.header;
world_header.stamp = sync_stamp;
world_header.frame_id = odom_msg->header.frame_id.empty()
? std::string("odom")
: odom_msg->header.frame_id;
if (target_observation.valid) {
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();
auto build_marker_array = [&active_tracks](
const std_msgs::msg::Header& header, bool world_frame) {
visualization_msgs::msg::MarkerArray out;
out.markers.reserve(1 + active_tracks.size() * 2);
// DELETEALL first clears any leftover markers from prior frames.
visualization_msgs::msg::Marker clear_all;
clear_all.header = header;
clear_all.action = visualization_msgs::msg::Marker::DELETEALL;
out.markers.push_back(clear_all);
for (const auto& obs : active_tracks) {
if (!obs.valid) continue;
const auto& pos = world_frame ? obs.detection_pos_world
: obs.detection_pos_cam;
visualization_msgs::msg::Marker sphere;
sphere.header = header;
sphere.ns = "detection_sphere";
sphere.id = obs.track_id;
sphere.type = visualization_msgs::msg::Marker::SPHERE;
sphere.action = visualization_msgs::msg::Marker::ADD;
sphere.pose.position.x = pos.x();
sphere.pose.position.y = pos.y();
sphere.pose.position.z = pos.z();
sphere.pose.orientation.w = 1.0;
sphere.scale.x = 0.3;
sphere.scale.y = 0.3;
sphere.scale.z = 0.3;
if (obs.is_primary_target) {
sphere.color.r = 1.0f;
sphere.color.g = 0.2f;
sphere.color.b = 0.2f;
} else {
pos_world_msg.point.x = -1.0;
pos_world_msg.point.y = -1.0;
pos_world_msg.point.z = -1.0;
sphere.color.r = 1.0f;
sphere.color.g = 0.95f;
sphere.color.b = 0.2f;
}
target_pos_world_pub_->publish(pos_world_msg);
sphere.color.a = 0.9f;
sphere.lifetime.sec = 0;
sphere.lifetime.nanosec = 300000000; // 0.3 s
out.markers.push_back(sphere);
visualization_msgs::msg::Marker label;
label.header = header;
label.ns = "detection_label";
label.id = obs.track_id;
label.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
label.action = visualization_msgs::msg::Marker::ADD;
label.pose.position.x = pos.x();
label.pose.position.y = pos.y();
label.pose.position.z = pos.z() + 0.5;
label.pose.orientation.w = 1.0;
label.scale.z = 0.25;
label.color.r = 1.0f;
label.color.g = 1.0f;
label.color.b = 1.0f;
label.color.a = 1.0f;
label.lifetime.sec = 0;
label.lifetime.nanosec = 300000000;
label.text = (obs.is_primary_target ? "*id=" : "id=") +
std::to_string(obs.track_id);
out.markers.push_back(label);
}
return out;
};
detection_pos_cam_pub_->publish(build_marker_array(cam_header, false));
detection_pos_world_pub_->publish(build_marker_array(world_header, true));
}
#endif
src/target_observation_processing.cpp
+590 -327
View File
File diff suppressed because it is too large Load Diff