diff --git a/config/control_command.yaml b/config/control_command.yaml index 8717240..6e37354 100644 --- a/config/control_command.yaml +++ b/config/control_command.yaml @@ -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 diff --git a/include/cloud_reprojection_ros_node.hpp b/include/cloud_reprojection_ros_node.hpp index bbd3c47..7e3b40e 100644 --- a/include/cloud_reprojection_ros_node.hpp +++ b/include/cloud_reprojection_ros_node.hpp @@ -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 odom_sub_; message_filters::Subscriber wiwc_sub_; message_filters::Subscriber image_sub_; + message_filters::Subscriber compressed_image_sub_; - typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; - typedef message_filters::Synchronizer Sync; - std::shared_ptr sync_; + typedef message_filters::sync_policies::ApproximateTime RawSyncPolicy; + typedef message_filters::Synchronizer RawSync; + typedef message_filters::sync_policies::ApproximateTime CompressedSyncPolicy; + typedef message_filters::Synchronizer CompressedSync; + std::shared_ptr raw_sync_; + std::shared_ptr 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::SharedPtr sync_odom_pub_; rclcpp::Publisher::SharedPtr sync_wiwc_pub_; rclcpp::Publisher::SharedPtr sync_image_pub_; + rclcpp::Publisher::SharedPtr sync_image_compressed_pub_; rclcpp::Publisher::SharedPtr overlay_compressed_pub_; // optional if send_overlay_ rclcpp::Publisher::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 diff --git a/script/record_dataset.sh b/script/record_dataset.sh index 5c06cbc..07329e8 100755 --- a/script/record_dataset.sh +++ b/script/record_dataset.sh @@ -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 diff --git a/src/cloud_reprojection_ros.cpp b/src/cloud_reprojection_ros.cpp index 10eb52e..208cac6 100644 --- a/src/cloud_reprojection_ros.cpp +++ b/src/cloud_reprojection_ros.cpp @@ -25,6 +25,7 @@ limitations under the License. #include #include #include +#include #ifdef ROS2 #include @@ -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_ : "") << "\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(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( + 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( + 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(sync_cloud_topic_, 10); sync_cloud_slam_pub_ = this->create_publisher(sync_cloud_slam_topic_, 10); sync_odom_pub_ = this->create_publisher(sync_odom_topic_, 10); sync_wiwc_pub_ = this->create_publisher(sync_wiwc_topic_, 10); sync_image_pub_ = this->create_publisher(sync_image_topic_, 10); + if (camera_image_compressed_) { + sync_image_compressed_pub_ = + this->create_publisher(sync_image_compressed_topic_, 10); + } if (send_overlay_) { overlay_compressed_pub_ = @@ -207,6 +249,7 @@ void CloudReprojectionRosNode::loadParameters() this->declare_parameter("odometry_topic", "/odin1/odometry"); this->declare_parameter("wiwc_topic", "/odin1/wiwc"); this->declare_parameter("register_keys.sync_camera_topic", "/odin1/image"); + this->declare_parameter("register_keys.sync_camera_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); @@ -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(image_msg->data.size()), + CV_8UC1, + const_cast(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) {