From a3fdce994204cd005f9993bed4e84359041cf1d2 Mon Sep 17 00:00:00 2001 From: Ray Ferric <63957587+rayferric@users.noreply.github.com> Date: Wed, 27 Mar 2024 13:32:00 +0100 Subject: [PATCH] Add an option to subscribe to compressed image topics. (#28) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Add an option to subscribe compressed image topics Also allow for relative names in cam_base_topic * Fix code style divergence reported by colcon test * Properly forward the original header when decompressing image messages. * Update aruco_opencv/src/aruco_tracker.cpp Co-authored-by: Błażej Sowa * Update aruco_opencv/src/aruco_tracker.cpp Co-authored-by: Błażej Sowa * Restructure image callbacks * Remove the unnecessary 'copy elision' comment * Reset compressed_img_sub_ in on_shutdown() --------- Co-authored-by: Błażej Sowa (cherry picked from commit 1150e04599bdc6896ec211c291493bff3f3b4dcb) --- aruco_opencv/config/aruco_tracker.yaml | 1 + aruco_opencv/src/aruco_tracker.cpp | 78 ++++++++++++++++++++------ 2 files changed, 63 insertions(+), 16 deletions(-) diff --git a/aruco_opencv/config/aruco_tracker.yaml b/aruco_opencv/config/aruco_tracker.yaml index eb53b1f..7931e35 100644 --- a/aruco_opencv/config/aruco_tracker.yaml +++ b/aruco_opencv/config/aruco_tracker.yaml @@ -5,6 +5,7 @@ marker_dict: 4X4_50 + image_sub_compressed: false image_sub_qos: reliability: 2 # 0 - system default, 1 - reliable, 2 - best effort durability: 2 # 0 - system default, 1 - transient local, 2 - volatile diff --git a/aruco_opencv/src/aruco_tracker.cpp b/aruco_opencv/src/aruco_tracker.cpp index aa482f9..9ea9f2f 100644 --- a/aruco_opencv/src/aruco_tracker.cpp +++ b/aruco_opencv/src/aruco_tracker.cpp @@ -59,6 +59,7 @@ class ArucoTracker : public rclcpp_lifecycle::LifecycleNode bool transform_poses_; bool publish_tf_; double marker_size_; + bool image_sub_compressed_; int image_sub_qos_reliability_; int image_sub_qos_durability_; int image_sub_qos_depth_; @@ -72,8 +73,10 @@ class ArucoTracker : public rclcpp_lifecycle::LifecycleNode rclcpp_lifecycle::LifecyclePublisher::SharedPtr debug_pub_; rclcpp::Subscription::SharedPtr cam_info_sub_; rclcpp::Subscription::SharedPtr img_sub_; + rclcpp::Subscription::SharedPtr compressed_img_sub_; rclcpp::Time last_msg_stamp_; bool cam_info_retrieved_ = false; + rclcpp::Time callback_start_time_; // Aruco cv::Mat camera_matrix_; @@ -157,7 +160,10 @@ class ArucoTracker : public rclcpp_lifecycle::LifecycleNode cam_info_retrieved_ = false; - std::string cam_info_topic = image_transport::getCameraInfoTopic(cam_base_topic_); + std::string image_topic = rclcpp::expand_topic_or_service_name( + cam_base_topic_, this->get_name(), this->get_namespace()); + std::string cam_info_topic = image_transport::getCameraInfoTopic(image_topic); + cam_info_sub_ = create_subscription( cam_info_topic, 1, std::bind(&ArucoTracker::callback_camera_info, this, std::placeholders::_1)); @@ -170,9 +176,15 @@ class ArucoTracker : public rclcpp_lifecycle::LifecycleNode auto qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(image_sub_qos), image_sub_qos); - img_sub_ = create_subscription( - cam_base_topic_, qos, std::bind( - &ArucoTracker::callback_image, this, std::placeholders::_1)); + if (image_sub_compressed_) { + compressed_img_sub_ = create_subscription( + image_topic + "/compressed", qos, std::bind( + &ArucoTracker::callback_compressed_image, this, std::placeholders::_1)); + } else { + img_sub_ = create_subscription( + image_topic, qos, std::bind( + &ArucoTracker::callback_image, this, std::placeholders::_1)); + } return LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -184,6 +196,7 @@ class ArucoTracker : public rclcpp_lifecycle::LifecycleNode on_set_parameter_callback_handle_.reset(); cam_info_sub_.reset(); img_sub_.reset(); + compressed_img_sub_.reset(); tf_listener_.reset(); tf_buffer_.reset(); @@ -213,6 +226,7 @@ class ArucoTracker : public rclcpp_lifecycle::LifecycleNode on_set_parameter_callback_handle_.reset(); cam_info_sub_.reset(); img_sub_.reset(); + compressed_img_sub_.reset(); tf_listener_.reset(); tf_buffer_.reset(); tf_broadcaster_.reset(); @@ -230,6 +244,7 @@ class ArucoTracker : public rclcpp_lifecycle::LifecycleNode declare_param(*this, "image_is_rectified", false, false); declare_param(*this, "output_frame", ""); declare_param(*this, "marker_dict", "4X4_50"); + declare_param(*this, "image_sub_compressed", false); declare_param( *this, "image_sub_qos.reliability", static_cast(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT)); @@ -265,6 +280,8 @@ class ArucoTracker : public rclcpp_lifecycle::LifecycleNode get_param(*this, "marker_dict", marker_dict_, "Marker Dictionary name: "); + get_parameter("image_sub_compressed", image_sub_compressed_); + get_parameter("image_sub_qos.reliability", image_sub_qos_reliability_); get_parameter("image_sub_qos.durability", image_sub_qos_durability_); get_parameter("image_sub_qos.depth", image_sub_qos_depth_); @@ -407,27 +424,53 @@ class ArucoTracker : public rclcpp_lifecycle::LifecycleNode } } + void callback_compressed_image(const sensor_msgs::msg::CompressedImage::ConstSharedPtr img_msg) + { + if (!should_process_img_msg(img_msg)) { + return; + } + + auto cv_ptr = cv_bridge::toCvCopy(img_msg, "bgr8"); + process_image(cv_ptr); + } + void callback_image(const sensor_msgs::msg::Image::ConstSharedPtr img_msg) + { + if (!should_process_img_msg(img_msg)) { + return; + } + + auto cv_ptr = cv_bridge::toCvShare(img_msg); + process_image(cv_ptr); + } + + template + bool should_process_img_msg(ImgMsgT img_msg) { RCLCPP_DEBUG_STREAM(get_logger(), "Image message address [SUBSCRIBE]:\t" << img_msg.get()); if (!cam_info_retrieved_) { - return; + RCLCPP_DEBUG(get_logger(), "Camera info not retrieved yet. Ignoring image..."); + return false; } if (img_msg->header.stamp == last_msg_stamp_) { RCLCPP_DEBUG( get_logger(), "The new image has the same timestamp as the previous one (duplicate frame?). Ignoring..."); - return; + return false; } + last_msg_stamp_ = img_msg->header.stamp; - auto callback_start_time = get_clock()->now(); + // We're ready to go, remember the current time to measure callback performance. + callback_start_time_ = get_clock()->now(); - // Convert the image - auto cv_ptr = cv_bridge::toCvShare(img_msg); + return true; + } + void process_image(const cv_bridge::CvImageConstPtr & cv_ptr) + { std::vector marker_ids; std::vector> marker_corners; @@ -440,8 +483,8 @@ class ArucoTracker : public rclcpp_lifecycle::LifecycleNode std::vector rvec_final(n_markers), tvec_final(n_markers); aruco_opencv_msgs::msg::ArucoDetection detection; - detection.header.frame_id = img_msg->header.frame_id; - detection.header.stamp = img_msg->header.stamp; + detection.header.frame_id = cv_ptr->header.frame_id; + detection.header.stamp = cv_ptr->header.stamp; detection.markers.resize(n_markers); { @@ -488,8 +531,8 @@ class ArucoTracker : public rclcpp_lifecycle::LifecycleNode // Retrieve camera -> output_frame transform try { cam_to_output = tf_buffer_->lookupTransform( - output_frame_, img_msg->header.frame_id, - img_msg->header.stamp, rclcpp::Duration::from_seconds(1.0)); + output_frame_, cv_ptr->header.frame_id, + cv_ptr->header.stamp, rclcpp::Duration::from_seconds(1.0)); } catch (tf2::TransformException & ex) { RCLCPP_ERROR_STREAM(get_logger(), ex.what()); return; @@ -530,7 +573,10 @@ class ArucoTracker : public rclcpp_lifecycle::LifecycleNode detection_pub_->publish(detection); if (debug_pub_->get_subscription_count() > 0) { - auto debug_cv_ptr = cv_bridge::toCvCopy(img_msg, "bgr8"); + auto debug_cv_ptr = std::make_shared(); + debug_cv_ptr->header = cv_ptr->header; + debug_cv_ptr->encoding = cv_ptr->encoding; + debug_cv_ptr->image = cv_ptr->image.clone(); cv::aruco::drawDetectedMarkers(debug_cv_ptr->image, marker_corners, marker_ids); { std::lock_guard guard(cam_info_mutex_); @@ -547,8 +593,8 @@ class ArucoTracker : public rclcpp_lifecycle::LifecycleNode } auto callback_end_time = get_clock()->now(); - double whole_callback_duration = (callback_end_time - callback_start_time).seconds(); - double image_send_duration = (callback_start_time - img_msg->header.stamp).seconds(); + double whole_callback_duration = (callback_end_time - callback_start_time_).seconds(); + double image_send_duration = (callback_start_time_ - cv_ptr->header.stamp).seconds(); RCLCPP_DEBUG( get_logger(), "Image callback completed. The callback started %.4f s after the image"