From ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Wed, 2 Oct 2024 21:01:34 +0200 Subject: [PATCH] Apply remappings to base topic before creating transport-specific topics (#326) --- image_transport/src/camera_publisher.cpp | 5 +---- image_transport/src/camera_subscriber.cpp | 4 +--- image_transport/src/publisher.cpp | 5 +---- image_transport/src/republish.cpp | 19 ++++++------------- image_transport/src/subscriber.cpp | 18 ++++++++---------- 5 files changed, 17 insertions(+), 34 deletions(-) diff --git a/image_transport/src/camera_publisher.cpp b/image_transport/src/camera_publisher.cpp index 66c7d7f1..09084e2f 100644 --- a/image_transport/src/camera_publisher.cpp +++ b/image_transport/src/camera_publisher.cpp @@ -32,7 +32,6 @@ #include #include -#include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/node.hpp" @@ -84,9 +83,7 @@ CameraPublisher::CameraPublisher( { // Explicitly resolve name here so we compute the correct CameraInfo topic when the // image topic is remapped (#4539). - std::string image_topic = rclcpp::expand_topic_or_service_name( - base_topic, - node->get_name(), node->get_namespace()); + std::string image_topic = node->get_node_topics_interface()->resolve_topic_name(base_topic); std::string info_topic = getCameraInfoTopic(image_topic); auto qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(custom_qos), custom_qos); diff --git a/image_transport/src/camera_subscriber.cpp b/image_transport/src/camera_subscriber.cpp index f4834e8a..aa1e6745 100644 --- a/image_transport/src/camera_subscriber.cpp +++ b/image_transport/src/camera_subscriber.cpp @@ -116,9 +116,7 @@ CameraSubscriber::CameraSubscriber( { // Must explicitly remap the image topic since we then do some string manipulation on it // to figure out the sibling camera_info topic. - std::string image_topic = rclcpp::expand_topic_or_service_name( - base_topic, - node->get_name(), node->get_namespace()); + std::string image_topic = node->get_node_topics_interface()->resolve_topic_name(base_topic); std::string info_topic = getCameraInfoTopic(image_topic); impl_->image_sub_.subscribe(node, image_topic, transport, custom_qos); diff --git a/image_transport/src/publisher.cpp b/image_transport/src/publisher.cpp index d74c5f8a..73beb2dc 100644 --- a/image_transport/src/publisher.cpp +++ b/image_transport/src/publisher.cpp @@ -35,7 +35,6 @@ #include #include -#include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/node.hpp" @@ -105,9 +104,7 @@ Publisher::Publisher( { // Resolve the name explicitly because otherwise the compressed topics don't remap // properly (#3652). - std::string image_topic = rclcpp::expand_topic_or_service_name( - base_topic, - node->get_name(), node->get_namespace()); + std::string image_topic = node->get_node_topics_interface()->resolve_topic_name(base_topic); impl_->base_topic_ = image_topic; impl_->loader_ = loader; diff --git a/image_transport/src/republish.cpp b/image_transport/src/republish.cpp index 849c5d0b..e0e2ea80 100644 --- a/image_transport/src/republish.cpp +++ b/image_transport/src/republish.cpp @@ -61,13 +61,6 @@ Republisher::Republisher(const rclcpp::NodeOptions & options) void Republisher::initialize() { - std::string in_topic = rclcpp::expand_topic_or_service_name( - "in", - this->get_name(), this->get_namespace()); - std::string out_topic = rclcpp::expand_topic_or_service_name( - "out", - this->get_name(), this->get_namespace()); - std::string in_transport = "raw"; this->declare_parameter("in_transport", in_transport); if (!this->get_parameter( @@ -117,14 +110,14 @@ void Republisher::initialize() PublishMemFn pub_mem_fn = &image_transport::Publisher::publish; pub_options.event_callbacks.matched_callback = - [this, in_topic, in_transport, pub_mem_fn, sub_options](rclcpp::MatchedInfo &) + [this, in_transport, pub_mem_fn, sub_options](rclcpp::MatchedInfo &) { std::scoped_lock lock(this->pub_matched_mutex); if (this->pub.getNumSubscribers() == 0) { this->sub.shutdown(); } else if (!this->sub) { this->sub = image_transport::create_subscription( - this, in_topic, + this, "in", std::bind(pub_mem_fn, &this->pub, std::placeholders::_1), in_transport, rmw_qos_profile_default, @@ -133,7 +126,7 @@ void Republisher::initialize() }; this->pub = image_transport::create_publisher( - this, out_topic, + this, "out", rmw_qos_profile_default, pub_options); } else { // Use one specific transport for output @@ -151,13 +144,13 @@ void Republisher::initialize() this->instance = loader->createUniqueInstance(lookup_name); pub_options.event_callbacks.matched_callback = - [this, in_topic, in_transport, pub_mem_fn, sub_options](rclcpp::MatchedInfo & matched_info) + [this, in_transport, pub_mem_fn, sub_options](rclcpp::MatchedInfo & matched_info) { if (matched_info.current_count == 0) { this->sub.shutdown(); } else if (!this->sub) { this->sub = image_transport::create_subscription( - this, in_topic, + this, "in", std::bind( pub_mem_fn, this->instance.get(), std::placeholders::_1), in_transport, rmw_qos_profile_default, @@ -165,7 +158,7 @@ void Republisher::initialize() } }; - this->instance->advertise(this, out_topic, rmw_qos_profile_default, pub_options); + this->instance->advertise(this, "out", rmw_qos_profile_default, pub_options); } } diff --git a/image_transport/src/subscriber.cpp b/image_transport/src/subscriber.cpp index 85c7927c..21835753 100644 --- a/image_transport/src/subscriber.cpp +++ b/image_transport/src/subscriber.cpp @@ -32,7 +32,6 @@ #include #include -#include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/logging.hpp" #include "sensor_msgs/msg/image.hpp" @@ -99,18 +98,17 @@ Subscriber::Subscriber( throw TransportLoadException(impl_->lookup_name_, e.what()); } + std::string image_topic = node->get_node_topics_interface()->resolve_topic_name(base_topic); + // Try to catch if user passed in a transport-specific topic as base_topic. - // TODO(ros2) use rclcpp to clean - // std::string clean_topic = ros::names::clean(base_topic); - std::string clean_topic = base_topic; - size_t found = clean_topic.rfind('/'); + size_t found = image_topic.rfind('/'); if (found != std::string::npos) { - std::string transport = clean_topic.substr(found + 1); + std::string transport = image_topic.substr(found + 1); std::string plugin_name = SubscriberPlugin::getLookupName(transport); std::vector plugins = loader->getDeclaredClasses(); if (std::find(plugins.begin(), plugins.end(), plugin_name) != plugins.end()) { - std::string real_base_topic = clean_topic.substr(0, found); + std::string real_base_topic = image_topic.substr(0, found); RCLCPP_WARN( impl_->logger_, @@ -119,13 +117,13 @@ Subscriber::Subscriber( "error. Try subscribing to the base topic '%s' instead with parameter ~image_transport " "set to '%s' (on the command line, _image_transport:=%s). " "See http://ros.org/wiki/image_transport for details.", - clean_topic.c_str(), real_base_topic.c_str(), transport.c_str(), transport.c_str()); + image_topic.c_str(), real_base_topic.c_str(), transport.c_str(), transport.c_str()); } } // Tell plugin to subscribe. - RCLCPP_DEBUG(impl_->logger_, "Subscribing to: %s\n", base_topic.c_str()); - impl_->subscriber_->subscribe(node, base_topic, callback, custom_qos, options); + RCLCPP_DEBUG(impl_->logger_, "Subscribing to: %s\n", image_topic.c_str()); + impl_->subscriber_->subscribe(node, image_topic, callback, custom_qos, options); } std::string Subscriber::getTopic() const