From d2a37a28ddb38a8c67c4e5b2f1343ab393b2bd89 Mon Sep 17 00:00:00 2001 From: hjy <1178065793@qq.com> Date: Tue, 21 Apr 2026 00:46:37 +0800 Subject: [PATCH] some configs in synced process dataset --- .gitignore | 3 +- .../odin1_reprojection_synced_ros2.launch.py | 3 +- src/cloud_reprojection_synced_ros.cpp | 32 ++++++++++++++++++- 3 files changed, 35 insertions(+), 3 deletions(-) diff --git a/.gitignore b/.gitignore index 531fb43..536f697 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ recorddata/ # /config/calib.yaml /log -/map \ No newline at end of file +/map +*.pyc diff --git a/launch_ROS2/odin1_reprojection_synced_ros2.launch.py b/launch_ROS2/odin1_reprojection_synced_ros2.launch.py index 8fb6315..b226bcc 100644 --- a/launch_ROS2/odin1_reprojection_synced_ros2.launch.py +++ b/launch_ROS2/odin1_reprojection_synced_ros2.launch.py @@ -19,7 +19,8 @@ def create_nodes(context): reprojection_params.setdefault('synced_odometry_topic', '/odin1/sync/odometry') reprojection_params.setdefault('synced_wiwc_topic', '/odin1/sync/wiwc') reprojection_params.setdefault('synced_image_topic', '/odin1/sync/image') - reprojection_params.setdefault('synced_image_compressed', 1) + sync_camera_compressed = int(reprojection_params.get('register_keys.sync_camera_compressed', 0)) + reprojection_params['synced_image_compressed'] = sync_camera_compressed reprojection_params.setdefault('register_keys.sync_topic_prefix', '/odin1/sync') reprojection_params['calib_file_path'] = os.path.join(package_dir, 'config', 'calib.yaml') diff --git a/src/cloud_reprojection_synced_ros.cpp b/src/cloud_reprojection_synced_ros.cpp index 805f789..2d9d74b 100644 --- a/src/cloud_reprojection_synced_ros.cpp +++ b/src/cloud_reprojection_synced_ros.cpp @@ -291,6 +291,9 @@ public: << "\n synced_wiwc_topic: " << synced_wiwc_topic_ << "\n synced_image_topic: " << synced_image_topic_ << "\n synced_image_transport: " << (synced_image_compressed_ ? "compressed" : "raw") + << "\n always_send_sync_compressed: " << (always_send_sync_compressed_ ? "on" : "off") + << "\n sync_image_compressed_topic: " + << ((!synced_image_compressed_ && always_send_sync_compressed_) ? sync_image_compressed_topic_ : "") << "\n output_prefix: " << sync_topic_prefix_ << "\n overlay_compressed_topic: " << sync_overlay_image_topic_ << "\n detection_debug_topic: " << sync_detection_debug_image_topic_ @@ -333,9 +336,11 @@ private: std::string synced_image_topic_; bool synced_image_compressed_ = true; std::string sync_topic_prefix_; + std::string sync_image_compressed_topic_; std::string combined_compressed_topic_; int combined_jpeg_quality_ = 85; int overlay_jpeg_quality_ = 85; + bool always_send_sync_compressed_ = false; bool publish_combined_compressed_ = true; bool send_overlay_ = true; @@ -353,6 +358,7 @@ private: rclcpp::Subscription::SharedPtr image_sub_; rclcpp::Subscription::SharedPtr compressed_image_sub_; + rclcpp::Publisher::SharedPtr sync_image_compressed_pub_; rclcpp::Publisher::SharedPtr overlay_compressed_pub_; rclcpp::Publisher::SharedPtr detection_debug_compressed_pub_; rclcpp::Publisher::SharedPtr gallery_debug_pub_; @@ -389,6 +395,8 @@ private: this->declare_parameter("synced_wiwc_topic", "/odin1/sync/wiwc"); this->declare_parameter("synced_image_topic", "/odin1/sync/image"); this->declare_parameter("synced_image_compressed", 1); + this->declare_parameter("register_keys.sync_camera_compressed", 0); + this->declare_parameter("register_keys.always_send_sync_compressed", 0); this->declare_parameter("register_keys.sync_topic_prefix", "/odin1/sync"); this->declare_parameter("register_keys.combined_compressed_topic", "/odin1/combined_image/compressed"); this->declare_parameter("register_keys.combined_jpeg_quality", 85); @@ -436,11 +444,15 @@ private: synced_cloud_topic_ = this->get_parameter("synced_cloud_topic").as_string(); synced_odometry_topic_ = this->get_parameter("synced_odometry_topic").as_string(); synced_wiwc_topic_ = this->get_parameter("synced_wiwc_topic").as_string(); - synced_image_compressed_ = (this->get_parameter("synced_image_compressed").as_int() != 0); + synced_image_compressed_ = + (this->get_parameter("register_keys.sync_camera_compressed").as_int() != 0); synced_image_topic_ = resolve_camera_topic( this->get_parameter("synced_image_topic").as_string(), synced_image_compressed_); + always_send_sync_compressed_ = + (this->get_parameter("register_keys.always_send_sync_compressed").as_int() != 0); sync_topic_prefix_ = this->get_parameter("register_keys.sync_topic_prefix").as_string(); + sync_image_compressed_topic_ = sync_topic_prefix_ + "/image/compressed"; combined_compressed_topic_ = this->get_parameter("register_keys.combined_compressed_topic").as_string(); combined_jpeg_quality_ = std::clamp( static_cast(this->get_parameter("register_keys.combined_jpeg_quality").as_int()), @@ -588,6 +600,10 @@ private: overlay_compressed_pub_ = this->create_publisher(sync_overlay_image_topic_, 10); } + if (!synced_image_compressed_ && always_send_sync_compressed_) { + sync_image_compressed_pub_ = + this->create_publisher(sync_image_compressed_topic_, 10); + } if (publish_combined_compressed_) { combined_pub_ = this->create_publisher(combined_compressed_topic_, 10); @@ -748,6 +764,20 @@ private: } } + if (!synced_image_compressed_ && always_send_sync_compressed_ && sync_image_compressed_pub_) { + std::vector buf; + const std::vector enc_params = {cv::IMWRITE_JPEG_QUALITY, overlay_jpeg_quality_}; + if (!cv::imencode(".jpg", cam_bgr, buf, enc_params)) { + RCLCPP_ERROR(this->get_logger(), "cv::imencode failed (synced image/compressed)"); + return; + } + CompressedImage sync_compressed_msg; + sync_compressed_msg.header = sync_image_msg.header; + sync_compressed_msg.format = "jpeg"; + sync_compressed_msg.data.assign(buf.begin(), buf.end()); + sync_image_compressed_pub_->publish(sync_compressed_msg); + } + pcl::PointCloud cloud_cam; pcl::fromROSMsg(*cloud_msg, cloud_cam); if (cloud_cam.empty()) {