Skip to content

Commit

Permalink
Add an option to subscribe to compressed image topics. (#28)
Browse files Browse the repository at this point in the history 
* 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 <[email protected]>

* Update aruco_opencv/src/aruco_tracker.cpp

Co-authored-by: Błażej Sowa <[email protected]>

* Restructure image callbacks

* Remove the unnecessary 'copy elision' comment

* Reset compressed_img_sub_ in on_shutdown()

---------

Co-authored-by: Błażej Sowa <[email protected]>
  • Loading branch information
@rayferric @bjsowa
rayferric and bjsowa authored Mar 27, 2024
1 parent 7df5e19 commit 1150e04
Show file tree
Hide file tree
Showing 2 changed files with 63 additions and 16 deletions.
1 change: 1 addition & 0 deletions aruco_opencv/config/aruco_tracker.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
78 changes: 62 additions & 16 deletions aruco_opencv/src/aruco_tracker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,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_;
Expand All @@ -73,8 +74,10 @@ class ArucoTracker : public rclcpp_lifecycle::LifecycleNode
rclcpp_lifecycle::LifecyclePublisher<sensor_msgs::msg::Image>::SharedPtr debug_pub_;
rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr cam_info_sub_;
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr img_sub_;
rclcpp::Subscription<sensor_msgs::msg::CompressedImage>::SharedPtr compressed_img_sub_;
rclcpp::Time last_msg_stamp_;
bool cam_info_retrieved_ = false;
rclcpp::Time callback_start_time_;

// Aruco
cv::Mat camera_matrix_;
Expand Down Expand Up @@ -158,7 +161,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<sensor_msgs::msg::CameraInfo>(
cam_info_topic, 1,
std::bind(&ArucoTracker::callback_camera_info, this, std::placeholders::_1));
Expand All @@ -171,9 +177,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<sensor_msgs::msg::Image>(
cam_base_topic_, qos, std::bind(
&ArucoTracker::callback_image, this, std::placeholders::_1));
if (image_sub_compressed_) {
compressed_img_sub_ = create_subscription<sensor_msgs::msg::CompressedImage>(
image_topic + "/compressed", qos, std::bind(
&ArucoTracker::callback_compressed_image, this, std::placeholders::_1));
} else {
img_sub_ = create_subscription<sensor_msgs::msg::Image>(
image_topic, qos, std::bind(
&ArucoTracker::callback_image, this, std::placeholders::_1));
}

return LifecycleNodeInterface::CallbackReturn::SUCCESS;
}
Expand All @@ -185,6 +197,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();

Expand Down Expand Up @@ -214,6 +227,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();
Expand All @@ -231,6 +245,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<int>(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT));
Expand Down Expand Up @@ -266,6 +281,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_);
Expand Down Expand Up @@ -408,27 +425,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<typename ImgMsgT>
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<int> marker_ids;
std::vector<std::vector<cv::Point2f>> marker_corners;

Expand All @@ -441,8 +484,8 @@ class ArucoTracker : public rclcpp_lifecycle::LifecycleNode
std::vector<cv::Vec3d> 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);

{
Expand Down Expand Up @@ -489,8 +532,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;
Expand Down Expand Up @@ -531,7 +574,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<cv_bridge::CvImage>();
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<std::mutex> guard(cam_info_mutex_);
Expand All @@ -548,8 +594,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"
Expand Down

0 comments on commit 1150e04

Please sign in to comment.