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

add sync input compressed; sync output could still support tp and pa

Browse Source
This commit is contained in:
hjy
2026-04-19 01:40:42 +08:00
parent 890783547c
commit 983da3e207
4 changed files with 157 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
config/control_command.yaml
+4 -1
View File
@@ -72,8 +72,11 @@ register_keys:
overlay_output_topic: "/odin1/combined_image/compressed"
overlay_jpeg_quality: 85
# cloud_reprojection: 4th input = raw camera (sensor_msgs/Image bgr8), e.g. /odin1/image
# cloud_reprojection 第4路图像输入；raw / compressed 二选一，由 sync_camera_compressed 控制
# sync_camera_compressed = 0: sync_camera_topic 应该是 sensor_msgs/Image (bgr8)，例如 /odin1/image
# sync_camera_compressed = 1: sync_camera_topic 应该是 sensor_msgs/CompressedImage，例如 /odin1/image/compressed
sync_camera_topic: "/odin1/image"
sync_camera_compressed: 1 # sync_camera_topic would be default added /compressed
sync_topic_prefix: "/odin1/sync"
combined_compressed_topic: "/odin1/combined_image/compressed"
combined_jpeg_quality: 85
include/cloud_reprojection_ros_node.hpp
+23 -7
View File
@@ -66,6 +66,7 @@ private:
std::string odometry_topic_;
std::string wiwc_topic_;
std::string camera_image_topic_;
bool camera_image_compressed_ = false;
std::string sync_topic_prefix_;
std::string combined_compressed_topic_;
int combined_jpeg_quality_;
@@ -77,16 +78,21 @@ private:
message_filters::Subscriber<Odometry> odom_sub_;
message_filters::Subscriber<Odometry> wiwc_sub_;
message_filters::Subscriber<Image> image_sub_;
message_filters::Subscriber<CompressedImage> compressed_image_sub_;
typedef message_filters::sync_policies::ApproximateTime<PointCloud2, Odometry, Odometry, Image> MySyncPolicy;
typedef message_filters::Synchronizer<MySyncPolicy> Sync;
std::shared_ptr<Sync> sync_;
typedef message_filters::sync_policies::ApproximateTime<PointCloud2, Odometry, Odometry, Image> RawSyncPolicy;
typedef message_filters::Synchronizer<RawSyncPolicy> RawSync;
typedef message_filters::sync_policies::ApproximateTime<PointCloud2, Odometry, Odometry, CompressedImage> CompressedSyncPolicy;
typedef message_filters::Synchronizer<CompressedSyncPolicy> CompressedSync;
std::shared_ptr<RawSync> raw_sync_;
std::shared_ptr<CompressedSync> compressed_sync_;
std::string sync_cloud_topic_;
std::string sync_cloud_slam_topic_;
std::string sync_odom_topic_;
std::string sync_wiwc_topic_;
std::string sync_image_topic_;
std::string sync_image_compressed_topic_;
std::string sync_overlay_image_topic_;
std::string sync_detection_debug_image_topic_;
std::string sync_target_observation_topic_;
@@ -98,6 +104,7 @@ private:
rclcpp::Publisher<Odometry>::SharedPtr sync_odom_pub_;
rclcpp::Publisher<Odometry>::SharedPtr sync_wiwc_pub_;
rclcpp::Publisher<Image>::SharedPtr sync_image_pub_;
rclcpp::Publisher<CompressedImage>::SharedPtr sync_image_compressed_pub_;
rclcpp::Publisher<CompressedImage>::SharedPtr overlay_compressed_pub_; // optional if send_overlay_
rclcpp::Publisher<CompressedImage>::SharedPtr detection_debug_compressed_pub_; // debug detections/tracks
@@ -114,10 +121,19 @@ private:
#endif
void loadParameters();
void syncCallback(const PointCloud2::ConstSharedPtr& cloud_msg,
const Odometry::ConstSharedPtr& odom_msg,
const Odometry::ConstSharedPtr& wiwc_msg,
const Image::ConstSharedPtr& image_msg);
void processSyncedData(const PointCloud2::ConstSharedPtr& cloud_msg,
const Odometry::ConstSharedPtr& odom_msg,
const Odometry::ConstSharedPtr& wiwc_msg,
const Image& image_msg,
const cv::Mat& cam_bgr);
void syncCallbackRaw(const PointCloud2::ConstSharedPtr& cloud_msg,
const Odometry::ConstSharedPtr& odom_msg,
const Odometry::ConstSharedPtr& wiwc_msg,
const Image::ConstSharedPtr& image_msg);
void syncCallbackCompressed(const PointCloud2::ConstSharedPtr& cloud_msg,
const Odometry::ConstSharedPtr& odom_msg,
const Odometry::ConstSharedPtr& wiwc_msg,
const CompressedImage::ConstSharedPtr& image_msg);
};
#else
class CloudReprojectionRosNode
script/record_dataset.sh
+1 -1
View File
@@ -3,5 +3,5 @@ ros2 bag record \
/odin1/sync/image/compressed \
/odin1/sync/odometry \
/odin1/sync/wiwc \
/odin1/combined_image/compressed
# /odin1/combined_image/compressed
src/cloud_reprojection_ros.cpp
+129 -30
View File
@@ -25,6 +25,7 @@ limitations under the License.
#include <chrono>
#include <vector>
#include <cstdint>
#include <opencv2/imgcodecs.hpp>
#ifdef ROS2
#include <sensor_msgs/msg/compressed_image.hpp>
@@ -86,6 +87,22 @@ std::string format_keypoints_xyc(
}
return oss.str();
}
std::string resolve_camera_sync_topic(std::string topic, bool compressed)
{
const std::string suffix = "/compressed";
if (compressed) {
if (topic.size() < suffix.size() ||
topic.compare(topic.size() - suffix.size(), suffix.size(), suffix) != 0) {
topic += suffix;
}
} else if (
topic.size() >= suffix.size() &&
topic.compare(topic.size() - suffix.size(), suffix.size(), suffix) == 0) {
topic.resize(topic.size() - suffix.size());
}
return topic;
}
} // namespace
#ifdef ROS2
@@ -129,6 +146,10 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op
<< "\n odometry_topic: " << odometry_topic_
<< "\n wiwc_topic: " << wiwc_topic_
<< "\n camera_image_topic: " << camera_image_topic_
<< "\n camera_image_transport: " << (camera_image_compressed_ ? "compressed" : "raw")
<< "\n sync_image_topic: " << sync_image_topic_
<< "\n sync_image_compressed_topic: "
<< (camera_image_compressed_ ? sync_image_compressed_topic_ : "<disabled>")
<< "\n sync_* topics under: " << sync_topic_prefix_
<< "\n overlay_compressed_topic: " << sync_overlay_image_topic_
<< "\n detection_debug_topic: " << sync_detection_debug_image_topic_
@@ -147,18 +168,39 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op
cloud_sub_.subscribe(this, cloud_slam_topic_);
odom_sub_.subscribe(this, odometry_topic_);
wiwc_sub_.subscribe(this, wiwc_topic_);
image_sub_.subscribe(this, camera_image_topic_);
sync_ = std::make_shared<Sync>(MySyncPolicy(10), cloud_sub_, odom_sub_, wiwc_sub_, image_sub_);
sync_->registerCallback(std::bind(&CloudReprojectionRosNode::syncCallback, this,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
std::placeholders::_4));
if (camera_image_compressed_) {
compressed_image_sub_.subscribe(this, camera_image_topic_);
compressed_sync_ = std::make_shared<CompressedSync>(
CompressedSyncPolicy(10), cloud_sub_, odom_sub_, wiwc_sub_, compressed_image_sub_);
compressed_sync_->registerCallback(std::bind(
&CloudReprojectionRosNode::syncCallbackCompressed,
this,
std::placeholders::_1,
std::placeholders::_2,
std::placeholders::_3,
std::placeholders::_4));
} else {
image_sub_.subscribe(this, camera_image_topic_);
raw_sync_ = std::make_shared<RawSync>(
RawSyncPolicy(10), cloud_sub_, odom_sub_, wiwc_sub_, image_sub_);
raw_sync_->registerCallback(std::bind(
&CloudReprojectionRosNode::syncCallbackRaw,
this,
std::placeholders::_1,
std::placeholders::_2,
std::placeholders::_3,
std::placeholders::_4));
}
sync_cloud_pub_ = this->create_publisher<PointCloud2>(sync_cloud_topic_, 10);
sync_cloud_slam_pub_ = this->create_publisher<PointCloud2>(sync_cloud_slam_topic_, 10);
sync_odom_pub_ = this->create_publisher<Odometry>(sync_odom_topic_, 10);
sync_wiwc_pub_ = this->create_publisher<Odometry>(sync_wiwc_topic_, 10);
sync_image_pub_ = this->create_publisher<Image>(sync_image_topic_, 10);
if (camera_image_compressed_) {
sync_image_compressed_pub_ =
this->create_publisher<CompressedImage>(sync_image_compressed_topic_, 10);
}
if (send_overlay_) {
overlay_compressed_pub_ =
@@ -207,6 +249,7 @@ void CloudReprojectionRosNode::loadParameters()
this->declare_parameter<std::string>("odometry_topic", "/odin1/odometry");
this->declare_parameter<std::string>("wiwc_topic", "/odin1/wiwc");
this->declare_parameter<std::string>("register_keys.sync_camera_topic", "/odin1/image");
this->declare_parameter<int>("register_keys.sync_camera_compressed", 0);
this->declare_parameter<std::string>("register_keys.sync_topic_prefix", "/odin1/sync");
this->declare_parameter<std::string>("register_keys.combined_compressed_topic", "/odin1/combined_image/compressed");
this->declare_parameter<int>("register_keys.combined_jpeg_quality", 85);
@@ -244,7 +287,11 @@ void CloudReprojectionRosNode::loadParameters()
cloud_slam_topic_ = this->get_parameter("cloud_slam_topic").as_string();
odometry_topic_ = this->get_parameter("odometry_topic").as_string();
wiwc_topic_ = this->get_parameter("wiwc_topic").as_string();
camera_image_topic_ = this->get_parameter("register_keys.sync_camera_topic").as_string();
camera_image_compressed_ =
(this->get_parameter("register_keys.sync_camera_compressed").as_int() != 0);
camera_image_topic_ = resolve_camera_sync_topic(
this->get_parameter("register_keys.sync_camera_topic").as_string(),
camera_image_compressed_);
sync_topic_prefix_ = this->get_parameter("register_keys.sync_topic_prefix").as_string();
combined_compressed_topic_ = this->get_parameter("register_keys.combined_compressed_topic").as_string();
combined_jpeg_quality_ = this->get_parameter("register_keys.combined_jpeg_quality").as_int();
@@ -260,6 +307,7 @@ void CloudReprojectionRosNode::loadParameters()
sync_odom_topic_ = sync_topic_prefix_ + "/odometry";
sync_wiwc_topic_ = sync_topic_prefix_ + "/wiwc";
sync_image_topic_ = sync_topic_prefix_ + "/image";
sync_image_compressed_topic_ = sync_topic_prefix_ + "/image/compressed";
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";
@@ -443,13 +491,14 @@ void CloudReprojectionRosNode::loadParameters()
#endif
}
void CloudReprojectionRosNode::syncCallback(
void CloudReprojectionRosNode::processSyncedData(
const PointCloud2::ConstSharedPtr& cloud_msg,
const Odometry::ConstSharedPtr& odom_msg,
const Odometry::ConstSharedPtr& wiwc_msg,
const Image::ConstSharedPtr& image_msg)
const Image& image_msg,
const cv::Mat& cam_bgr)
{
const auto sync_stamp = image_msg->header.stamp;
const auto sync_stamp = image_msg.header.stamp;
PointCloud2 sync_cloud_slam_msg = *cloud_msg;
sync_cloud_slam_msg.header.stamp = sync_stamp;
@@ -457,7 +506,7 @@ void CloudReprojectionRosNode::syncCallback(
sync_odom_msg.header.stamp = sync_stamp;
Odometry sync_wiwc_msg = *wiwc_msg;
sync_wiwc_msg.header.stamp = sync_stamp;
Image sync_image_msg = *image_msg;
Image sync_image_msg = image_msg;
sync_image_msg.header.stamp = sync_stamp;
@@ -508,7 +557,7 @@ void CloudReprojectionRosNode::syncCallback(
PointCloud2 cloud_cam_msg;
pcl::toROSMsg(cloud_cam, cloud_cam_msg);
cloud_cam_msg.header = image_msg->header;
cloud_cam_msg.header = image_msg.header;
cloud_cam_msg.header.stamp = sync_stamp;
if (cloud_cam_msg.header.frame_id.empty()) {
// cloud_cam_msg.header.frame_id = cloud_msg->header.frame_id;
@@ -525,19 +574,15 @@ void CloudReprojectionRosNode::syncCallback(
}
}
cv::Mat cam_bgr;
if (send_overlay_ || need_combined
const bool need_cam_bgr =
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;
} catch (const cv_bridge::Exception& e) {
RCLCPP_ERROR(this->get_logger(), "cv_bridge (sync image): %s", e.what());
return;
}
;
if (need_cam_bgr && cam_bgr.empty()) {
RCLCPP_ERROR(this->get_logger(), "Synced image decode/convert failed");
return;
}
sync_cloud_slam_pub_->publish(sync_cloud_slam_msg);
@@ -644,8 +689,8 @@ void CloudReprojectionRosNode::syncCallback(
*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",
image_msg->header.stamp.sec,
image_msg->header.stamp.nanosec,
image_msg.header.stamp.sec,
image_msg.header.stamp.nanosec,
cam_bgr.cols,
cam_bgr.rows,
cloud_cam.size(),
@@ -688,7 +733,7 @@ void CloudReprojectionRosNode::syncCallback(
}
odin_ros_driver::msg::TargetObservation observation_msg;
observation_msg.header = image_msg->header;
observation_msg.header = image_msg.header;
observation_msg.header.stamp = sync_stamp;
observation_msg.odometry = sync_odom_msg;
observation_msg.valid = target_observation.valid;
@@ -724,7 +769,7 @@ void CloudReprojectionRosNode::syncCallback(
target_observation_pub_->publish(observation_msg);
geometry_msgs::msg::PointStamped pos_cam_msg;
pos_cam_msg.header = image_msg->header;
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"
@@ -741,7 +786,7 @@ void CloudReprojectionRosNode::syncCallback(
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 = image_msg.header;
pos_world_msg.header.stamp = sync_stamp;
pos_world_msg.header.frame_id = odom_msg->header.frame_id.empty()
? "odom"
@@ -770,7 +815,7 @@ void CloudReprojectionRosNode::syncCallback(
return;
}
CompressedImage omsg;
omsg.header = image_msg->header;
omsg.header = image_msg.header;
omsg.format = "jpeg";
omsg.data.assign(obuf.begin(), obuf.end());
overlay_compressed_pub_->publish(omsg);
@@ -795,7 +840,7 @@ void CloudReprojectionRosNode::syncCallback(
return;
}
CompressedImage dmsg;
dmsg.header = image_msg->header;
dmsg.header = image_msg.header;
dmsg.format = "jpeg";
dmsg.data.assign(dbuf.begin(), dbuf.end());
detection_debug_compressed_pub_->publish(dmsg);
@@ -818,13 +863,67 @@ void CloudReprojectionRosNode::syncCallback(
}
CompressedImage out;
out.header = image_msg->header;
out.header = image_msg.header;
out.format = "jpeg";
out.data.assign(buf.begin(), buf.end());
combined_pub_->publish(out);
}
}
void CloudReprojectionRosNode::syncCallbackRaw(
const PointCloud2::ConstSharedPtr& cloud_msg,
const Odometry::ConstSharedPtr& odom_msg,
const Odometry::ConstSharedPtr& wiwc_msg,
const Image::ConstSharedPtr& image_msg)
{
cv::Mat cam_bgr;
const bool need_cam_bgr =
send_overlay_ || (combined_pub_ != nullptr)
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
|| enable_target_observation_
#endif
;
if (need_cam_bgr) {
try {
cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(image_msg, "bgr8");
cam_bgr = cv_ptr->image;
} catch (const cv_bridge::Exception& e) {
RCLCPP_ERROR(this->get_logger(), "cv_bridge (sync image): %s", e.what());
return;
}
}
processSyncedData(cloud_msg, odom_msg, wiwc_msg, *image_msg, cam_bgr);
}
void CloudReprojectionRosNode::syncCallbackCompressed(
const PointCloud2::ConstSharedPtr& cloud_msg,
const Odometry::ConstSharedPtr& odom_msg,
const Odometry::ConstSharedPtr& wiwc_msg,
const CompressedImage::ConstSharedPtr& image_msg)
{
if (image_msg->data.empty()) {
RCLCPP_WARN(this->get_logger(), "Empty compressed image received");
return;
}
const cv::Mat encoded(
1,
static_cast<int>(image_msg->data.size()),
CV_8UC1,
const_cast<unsigned char*>(image_msg->data.data()));
cv::Mat cam_bgr = cv::imdecode(encoded, cv::IMREAD_COLOR);
if (cam_bgr.empty()) {
RCLCPP_ERROR(this->get_logger(), "cv::imdecode failed (sync compressed image)");
return;
}
if (sync_image_compressed_pub_) {
CompressedImage sync_compressed_msg = *image_msg;
sync_image_compressed_pub_->publish(sync_compressed_msg);
}
auto sync_image_msg =
cv_bridge::CvImage(image_msg->header, "bgr8", cam_bgr).toImageMsg();
processSyncedData(cloud_msg, odom_msg, wiwc_msg, *sync_image_msg, cam_bgr);
}
// ==================== ROS2 Main ====================
int main(int argc, char** argv)
{