From 21844837c029629d9b044b0f52896d1456ed47cb Mon Sep 17 00:00:00 2001 From: Markus Hofstaetter Date: Wed, 5 May 2021 16:30:06 +0000 Subject: [PATCH] Fix resolving sub-namespaces incorrectly Signed-off-by: Markus Hofstaetter --- image_transport/src/camera_publisher.cpp | 8 +++++++- image_transport/src/camera_subscriber.cpp | 8 +++++++- image_transport/src/publisher.cpp | 8 +++++++- 3 files changed, 21 insertions(+), 3 deletions(-) diff --git a/image_transport/src/camera_publisher.cpp b/image_transport/src/camera_publisher.cpp index 8a6514a7..f846150a 100644 --- a/image_transport/src/camera_publisher.cpp +++ b/image_transport/src/camera_publisher.cpp @@ -87,8 +87,14 @@ CameraPublisher::CameraPublisher( { // Explicitly resolve name here so we compute the correct CameraInfo topic when the // image topic is remapped (#4539). + + // FIXME: once https://github.com/ros2/rclcpp/issues/1656 is resolved, see #187 + std::string effective_namespace = node->get_effective_namespace(); + if (effective_namespace.length() > 1 && effective_namespace.back() == '/') + effective_namespace.pop_back(); + std::string image_topic = rclcpp::expand_topic_or_service_name(base_topic, - node->get_name(), node->get_namespace()); + node->get_name(), effective_namespace); 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 4e804e25..ca15d066 100644 --- a/image_transport/src/camera_subscriber.cpp +++ b/image_transport/src/camera_subscriber.cpp @@ -120,8 +120,14 @@ 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. + + // FIXME: once https://github.com/ros2/rclcpp/issues/1656 is resolved, see #187 + std::string effective_namespace = node->get_effective_namespace(); + if (effective_namespace.length() > 1 && effective_namespace.back() == '/') + effective_namespace.pop_back(); + std::string image_topic = rclcpp::expand_topic_or_service_name(base_topic, - node->get_name(), node->get_namespace()); + node->get_name(), effective_namespace); 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 9d8d2831..079713ae 100644 --- a/image_transport/src/publisher.cpp +++ b/image_transport/src/publisher.cpp @@ -105,8 +105,14 @@ Publisher::Publisher( { // Resolve the name explicitly because otherwise the compressed topics don't remap // properly (#3652). + + // FIXME: once https://github.com/ros2/rclcpp/issues/1656 is resolved, see #187 + std::string effective_namespace = node->get_effective_namespace(); + if (effective_namespace.length() > 1 && effective_namespace.back() == '/') + effective_namespace.pop_back(); + std::string image_topic = rclcpp::expand_topic_or_service_name(base_topic, - node->get_name(), node->get_namespace()); + node->get_name(), effective_namespace); impl_->base_topic_ = image_topic; impl_->loader_ = loader;