From 682fe00d81daed042febc51fcf7bef58e5ee7579 Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Fri, 19 Jan 2024 09:17:01 -0500 Subject: [PATCH] image_rotate: clean up (#862) This is the first component/node with a cleanup pass to be fully implemented: * Fix #740 by initializing vectors. Do this by declaring parameters AFTER we define the callback * Implement lazy subscribers (I missed this in the earlier PRs) * Add image_transport parameter so we can specify that desired transport of our subscriptions * Update how we test for connectivity (and update the debug message - has nothing to do with whether we are remapped, it's really about whether we are connected) --- .../image_rotate/image_rotate_node.hpp | 4 - image_rotate/src/image_rotate.cpp | 7 - image_rotate/src/image_rotate_node.cpp | 178 ++++++++---------- 3 files changed, 80 insertions(+), 109 deletions(-) diff --git a/image_rotate/include/image_rotate/image_rotate_node.hpp b/image_rotate/include/image_rotate/image_rotate_node.hpp index 8482a1c72..67e08eae3 100644 --- a/image_rotate/include/image_rotate/image_rotate_node.hpp +++ b/image_rotate/include/image_rotate/image_rotate_node.hpp @@ -82,10 +82,6 @@ class ImageRotateNode : public rclcpp::Node void do_work( const sensor_msgs::msg::Image::ConstSharedPtr & msg, const std::string input_frame_from_msg); - void subscribe(); - void unsubscribe(); - void connectCb(); - void disconnectCb(); void onInit(); rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_handle_; diff --git a/image_rotate/src/image_rotate.cpp b/image_rotate/src/image_rotate.cpp index 909d34ef8..3d50de2c2 100644 --- a/image_rotate/src/image_rotate.cpp +++ b/image_rotate/src/image_rotate.cpp @@ -38,13 +38,6 @@ int main(int argc, char ** argv) // Force flush of the stdout buffer. setvbuf(stdout, NULL, _IONBF, BUFSIZ); - if (argc <= 1) { - RCUTILS_LOG_WARN( - "Topic 'image' has not been remapped! Typical command-line usage:\n" - "\t$ ros2 run image_rotate image_rotate image:="); - return 1; - } - rclcpp::init(argc, argv); rclcpp::NodeOptions options; auto node = std::make_shared(options); diff --git a/image_rotate/src/image_rotate_node.cpp b/image_rotate/src/image_rotate_node.cpp index 1935a9ec9..85a596edb 100644 --- a/image_rotate/src/image_rotate_node.cpp +++ b/image_rotate/src/image_rotate_node.cpp @@ -72,31 +72,9 @@ namespace image_rotate ImageRotateNode::ImageRotateNode(const rclcpp::NodeOptions & options) : rclcpp::Node("ImageRotateNode", options) { - config_.target_frame_id = this->declare_parameter("target_frame_id", std::string("")); - config_.target_x = this->declare_parameter("target_x", 0.0); - config_.target_y = this->declare_parameter("target_y", 0.0); - config_.target_z = this->declare_parameter("target_z", 1.0); - - config_.source_frame_id = this->declare_parameter("source_frame_id", std::string("")); - config_.source_x = this->declare_parameter("source_x", 0.0); - config_.source_y = this->declare_parameter("source_y", -1.0); - config_.source_z = this->declare_parameter("source_z", 0.0); - - config_.output_frame_id = this->declare_parameter("output_frame_id", std::string("")); - config_.input_frame_id = this->declare_parameter("input_frame_id", std::string("")); - config_.use_camera_info = this->declare_parameter("use_camera_info", true); - config_.max_angular_rate = this->declare_parameter( - "max_angular_rate", - 10.0); - config_.output_image_size = this->declare_parameter( - "output_image_size", - 2.0); - auto reconfigureCallback = [this](std::vector parameters) -> rcl_interfaces::msg::SetParametersResult { - RCLCPP_INFO(get_logger(), "reconfigureCallback"); - auto result = rcl_interfaces::msg::SetParametersResult(); result.successful = true; for (auto parameter : parameters) { @@ -128,13 +106,31 @@ ImageRotateNode::ImageRotateNode(const rclcpp::NodeOptions & options) source_vector_.vector.x = config_.source_x; source_vector_.vector.y = config_.source_y; source_vector_.vector.z = config_.source_z; - if (subscriber_count_) { // @todo: Could do this without an interruption at some point. - unsubscribe(); - subscribe(); - } + return result; }; on_set_parameters_callback_handle_ = this->add_on_set_parameters_callback(reconfigureCallback); + + // Set parameters AFTER add_on_set_parameters_callback + config_.target_frame_id = this->declare_parameter("target_frame_id", std::string("")); + config_.target_x = this->declare_parameter("target_x", 0.0); + config_.target_y = this->declare_parameter("target_y", 0.0); + config_.target_z = this->declare_parameter("target_z", 1.0); + + config_.source_frame_id = this->declare_parameter("source_frame_id", std::string("")); + config_.source_x = this->declare_parameter("source_x", 0.0); + config_.source_y = this->declare_parameter("source_y", -1.0); + config_.source_z = this->declare_parameter("source_z", 0.0); + + config_.output_frame_id = this->declare_parameter("output_frame_id", std::string("")); + config_.input_frame_id = this->declare_parameter("input_frame_id", std::string("")); + config_.use_camera_info = this->declare_parameter("use_camera_info", true); + config_.max_angular_rate = this->declare_parameter("max_angular_rate", 10.0); + config_.output_image_size = this->declare_parameter("output_image_size", 2.0); + + // TransportHints does not actually declare the parameter + this->declare_parameter("image_transport", "raw"); + onInit(); } @@ -187,13 +183,6 @@ void ImageRotateNode::do_work( source_frame_id, input_frame_id, tf2_time, tf2_time - prev_stamp_); tf2::doTransform(source_vector_, source_vector_transformed, transform); - // RCUTILS_LOG_INFO("target: %f %f %f", target_vector_.x, target_vector_.y, target_vector_.z); - // RCUTILS_LOG_INFO("target_transformed: %f %f %f", target_vector_transformed.x, " - // "target_vector_transformed.y, target_vector_transformed.z"); - // RCUTILS_LOG_INFO("source: %f %f %f", source_vector_.x, source_vector_.y, source_vector_.z); - // RCUTILS_LOG_INFO("source_transformed: %f %f %f", source_vector_transformed.x, " - // "source_vector_transformed.y, source_vector_transformed.z"); - // Calculate the angle of the rotation. double angle = angle_; if ((target_vector_transformed.vector.x != 0 || target_vector_transformed.vector.y != 0) && @@ -226,11 +215,9 @@ void ImageRotateNode::do_work( } angle_ = fmod(angle_, 2.0 * M_PI); } catch (const tf2::TransformException & e) { - RCUTILS_LOG_ERROR("Transform error: %s", e.what()); + RCLCPP_ERROR(get_logger(), "Transform error: %s", e.what()); } - // RCUTILS_LOG_INFO("angle: %f", 180 * angle_ / M_PI); - // Publish the transform. geometry_msgs::msg::TransformStamped transform; transform.transform.translation.x = 0; @@ -262,7 +249,6 @@ void ImageRotateNode::do_work( int step = config_.output_image_size; out_size = candidates[step] + (candidates[step + 1] - candidates[step]) * (config_.output_image_size - step); - // RCUTILS_LOG_INFO("out_size: %d", out_size); // Compute the rotation matrix. cv::Mat rot_matrix = cv::getRotationMatrix2D( @@ -281,7 +267,8 @@ void ImageRotateNode::do_work( out_img->header.frame_id = transform.child_frame_id; img_pub_.publish(out_img); } catch (const cv::Exception & e) { - RCUTILS_LOG_ERROR( + RCLCPP_ERROR( + get_logger(), "Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); } @@ -289,55 +276,6 @@ void ImageRotateNode::do_work( prev_stamp_ = tf2_ros::fromMsg(msg->header.stamp); } -void ImageRotateNode::subscribe() -{ - RCUTILS_LOG_DEBUG("Subscribing to image topic."); - if (config_.use_camera_info && config_.input_frame_id.empty()) { - auto custom_qos = rmw_qos_profile_system_default; - custom_qos.depth = 3; - - cam_sub_ = image_transport::create_camera_subscription( - this, - "image", - std::bind( - &ImageRotateNode::imageCallbackWithInfo, this, - std::placeholders::_1, std::placeholders::_2), - "raw", - custom_qos); - } else { - auto custom_qos = rmw_qos_profile_system_default; - custom_qos.depth = 3; - img_sub_ = image_transport::create_subscription( - this, - "image", - std::bind(&ImageRotateNode::imageCallback, this, std::placeholders::_1), - "raw", - custom_qos); - } -} - -void ImageRotateNode::unsubscribe() -{ - RCUTILS_LOG_DEBUG("Unsubscribing from image topic."); - img_sub_.shutdown(); - cam_sub_.shutdown(); -} - -void ImageRotateNode::connectCb() -{ - if (subscriber_count_++ == 0) { - subscribe(); - } -} - -void ImageRotateNode::disconnectCb() -{ - subscriber_count_--; - if (subscriber_count_ == 0) { - unsubscribe(); - } -} - void ImageRotateNode::onInit() { subscriber_count_ = 0; @@ -346,18 +284,62 @@ void ImageRotateNode::onInit() rclcpp::Clock::SharedPtr clock = this->get_clock(); tf_buffer_ = std::make_shared(clock); tf_sub_ = std::make_shared(*tf_buffer_); - // TODO(yechun1): Implement when SubscriberStatusCallback is available - // image_transport::SubscriberStatusCallback connect_cb = - // boost::bind(&ImageRotateNode::connectCb, this, _1); - // image_transport::SubscriberStatusCallback connect_cb = - // std::bind(&CropForemostNode::connectCb, this); - // image_transport::SubscriberStatusCallback disconnect_cb = - // boost::bind(&ImageRotateNode::disconnectCb, this, _1); - // img_pub_ = image_transport::ImageTransport(ros::NodeHandle(nh_, "rotated")).advertise( - // "image", 1, connect_cb, disconnect_cb); - connectCb(); - img_pub_ = image_transport::create_publisher(this, "rotated/image"); tf_pub_ = std::make_shared(*this); + + // Create publisher with connect callback + rclcpp::PublisherOptions pub_options; + pub_options.event_callbacks.matched_callback = + [this](rclcpp::MatchedInfo &) + { + if (img_pub_.getNumSubscribers() == 0) { + RCLCPP_DEBUG(get_logger(), "Unsubscribing from image topic."); + img_sub_.shutdown(); + cam_sub_.shutdown(); + } else { + // For compressed topics to remap appropriately, we need to pass a + // fully expanded and remapped topic name to image_transport + auto node_base = this->get_node_base_interface(); + std::string topic_name = node_base->resolve_topic_or_service_name("image", false); + RCLCPP_INFO(get_logger(), "Subscribing to %s topic.", topic_name.c_str()); + + // Check that remapping appears to be correct + auto topics = this->get_topic_names_and_types(); + if (topics.find(topic_name) == topics.end()) { + RCLCPP_WARN( + get_logger(), + "Topic %s is not connected! Typical command-line usage:\n" + "\t$ ros2 run image_rotate image_rotate --ros-args -r image:=", + topic_name.c_str()); + } + + // This will check image_transport parameter to get proper transport + image_transport::TransportHints transport_hint(this, "raw"); + + if (config_.use_camera_info && config_.input_frame_id.empty()) { + auto custom_qos = rmw_qos_profile_system_default; + custom_qos.depth = 3; + cam_sub_ = image_transport::create_camera_subscription( + this, + topic_name, + std::bind( + &ImageRotateNode::imageCallbackWithInfo, this, + std::placeholders::_1, std::placeholders::_2), + transport_hint.getTransport(), + custom_qos); + } else { + auto custom_qos = rmw_qos_profile_system_default; + custom_qos.depth = 3; + img_sub_ = image_transport::create_subscription( + this, + topic_name, + std::bind(&ImageRotateNode::imageCallback, this, std::placeholders::_1), + transport_hint.getTransport(), + custom_qos); + } + } + }; + img_pub_ = image_transport::create_publisher( + this, "rotated/image", rmw_qos_profile_default, pub_options); } } // namespace image_rotate