Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add an option to subscribe to compressed image topics. #28

Merged
merged 8 commits into from
Mar 27, 2024
Merged

Add an option to subscribe to compressed image topics. #28

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
6ad06c9
Add an option to subscribe compressed image topics
rayferric Mar 25, 2024
97d9fab
Fix code style divergence reported by colcon test
rayferric Mar 25, 2024
bacfd77
Properly forward the original header when decompressing image messages.
rayferric Mar 25, 2024
1437572
Update aruco_opencv/src/aruco_tracker.cpp
rayferric Mar 26, 2024
5f69eca
Update aruco_opencv/src/aruco_tracker.cpp
rayferric Mar 26, 2024
b29a6ab
Restructure image callbacks
rayferric Mar 26, 2024
4530210
Remove the unnecessary 'copy elision' comment
rayferric Mar 26, 2024
853e949
Reset compressed_img_sub_ in on_shutdown()
rayferric Mar 27, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions aruco_opencv/config/aruco_tracker.yaml
View file
Open in desktop
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
Open in desktop
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
Loading