From 596eb1c142d152403722c13ae9cf5db1cb4f1fb2 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= <ahcorde@gmail.com>
Date: Mon, 1 Apr 2024 09:16:18 +0200
Subject: [PATCH] [Backport rolling] Add ROS namespaces to GZ topics (#517)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: Kotochleb <krzy.wojciecho@gmail.com>
Signed-off-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com>
Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com>
---
 ros_gz_bridge/README.md             | 40 ++++++++++++++++++++++++++---
 ros_gz_bridge/src/ros_gz_bridge.cpp | 14 ++++++++--
 2 files changed, 49 insertions(+), 5 deletions(-)

diff --git a/ros_gz_bridge/README.md b/ros_gz_bridge/README.md
index dee6bdd8..57ae4598 100644
--- a/ros_gz_bridge/README.md
+++ b/ros_gz_bridge/README.md
@@ -102,7 +102,7 @@ Now we start the ROS listener.
 
 ```
 # Shell B:
-. /opt/ros/galactic/setup.bash
+. /opt/ros/rolling/setup.bash
 ros2 topic echo /chatter
 ```
 
@@ -134,7 +134,7 @@ Now we start the ROS talker.
 
 ```
 # Shell C:
-. /opt/ros/galactic/setup.bash
+. /opt/ros/rolling/setup.bash
 ros2 topic pub /chatter std_msgs/msg/String "data: 'Hi'" --once
 ```
 
@@ -172,7 +172,7 @@ Now we start the ROS GUI:
 
 ```
 # Shell C:
-. /opt/ros/galactic/setup.bash
+. /opt/ros/rolling/setup.bash
 ros2 run rqt_image_view rqt_image_view /rgbd_camera/image
 ```
 
@@ -290,9 +290,43 @@ To run the bridge node with the above configuration:
 ros2 run ros_gz_bridge parameter_bridge --ros-args -p config_file:=$WORKSPACE/ros_gz/ros_gz_bridge/test/config/full.yaml
 ```
 
+## Example 6: Using ROS namespace with the Bridge
+
+When spawning multiple robots inside the same ROS environment, it is convenient to use namespaces to avoid overlapping topic names.
+There are three main types of namespaces: relative, global (`/`) and private (`~/`). For more information, refer to ROS documentation.
+Namespaces are applied to Gazebo topic both when specified as `topic_name` as well as `gz_topic_name`.
+
+By default, the Bridge will not apply ROS namespace on the Gazebo topics. To enable this feature, use parameter `expand_gz_topic_names`.
+Let's test our topic with namespace:
+
+```bash
+# Shell A:
+. ~/bridge_ws/install/setup.bash
+ros2 run ros_gz_bridge parameter_bridge chatter@std_msgs/msg/String@ignition.msgs.StringMsg \
+  --ros-args -p expand_gz_topic_names:=true -r __ns:=/demo
+```
+
+Now we start the Gazebo Transport listener.
+
+```bash
+# Shell B:
+gz topic -e -t /demo/chatter
+```
+
+Now we start the ROS talker.
+
+```bash
+# Shell C:
+. /opt/ros/rolling/setup.bash
+ros2 topic pub /demo/chatter std_msgs/msg/String "data: 'Hi from inside of a namespace'" --once
+```
+
+By changing `chatter` to `/chatter` or `~/chatter` you can obtain different results.
+
 ## API
 
 ROS 2 Parameters:
 
  * `subscription_heartbeat` - Period at which the node checks for new subscribers for lazy bridges.
  * `config_file` - YAML file to be loaded as the bridge configuration
+ * `expand_gz_topic_names` - Enable or disable ROS namespace applied on GZ topics.
diff --git a/ros_gz_bridge/src/ros_gz_bridge.cpp b/ros_gz_bridge/src/ros_gz_bridge.cpp
index f542ee90..46a86d98 100644
--- a/ros_gz_bridge/src/ros_gz_bridge.cpp
+++ b/ros_gz_bridge/src/ros_gz_bridge.cpp
@@ -20,6 +20,8 @@
 #include "bridge_handle_ros_to_gz.hpp"
 #include "bridge_handle_gz_to_ros.hpp"
 
+#include <rclcpp/expand_topic_or_service_name.hpp>
+
 namespace ros_gz_bridge
 {
 
@@ -30,6 +32,7 @@ RosGzBridge::RosGzBridge(const rclcpp::NodeOptions & options)
 
   this->declare_parameter<int>("subscription_heartbeat", 1000);
   this->declare_parameter<std::string>("config_file", "");
+  this->declare_parameter<bool>("expand_gz_topic_names", false);
 
   int heartbeat;
   this->get_parameter("subscription_heartbeat", heartbeat);
@@ -43,14 +46,21 @@ void RosGzBridge::spin()
   if (handles_.empty()) {
     std::string config_file;
     this->get_parameter("config_file", config_file);
+    bool expand_names;
+    this->get_parameter("expand_gz_topic_names", expand_names);
+    const std::string ros_ns = this->get_namespace();
+    const std::string ros_node_name = this->get_name();
     if (!config_file.empty()) {
       auto entries = readFromYamlFile(config_file);
-      for (const auto & entry : entries) {
+      for (auto & entry : entries) {
+        if (expand_names) {
+          entry.gz_topic_name = rclcpp::expand_topic_or_service_name(
+            entry.gz_topic_name, ros_node_name, ros_ns, false);
+        }
         this->add_bridge(entry);
       }
     }
   }
-
   for (auto & bridge : handles_) {
     bridge->Spin();
   }