DTSW-5002 Remapped gazebo camera topics

Docker/duckietown/Dockerfile.gz-garden-bridge

@@ -36,5 +36,7 @@ RUN /bin/bash -c "source /opt/ros/humble/setup.bash && colcon build"
 
 # Copy the FastDDS configuration file and the Gazebo bridge configuration file
 COPY fastdds_no_shared_memory.xml /data/config/fastdds_no_shared_memory.xml
-COPY gazebo-bridge-config.yaml /data/config/gazebo-bridge-config.yaml
+RUN apt-get update && apt-get install -y python3-jinja2
+COPY gazebo-bridge-config.yaml.j2 /data/config/gazebo-bridge-config.yaml.j2
+COPY ./gazebo-bridge-replacer.py /data/config/gazebo-bridge-replacer.py
 

Docker/duckietown/Dockerfile.ros2-ros1-bridge

@@ -10,4 +10,6 @@ RUN apt-get update \
 CMD ["avahi-daemon", "--no-drop-root", "-D"]
 
 COPY fastdds_no_shared_memory.xml /data/config/fastdds_no_shared_memory.xml
-COPY bridge.yml /data/config/bridge.yml
+COPY bridge.yml.j2 /data/config/bridge.yml.j2
+RUN apt-get update && apt-get install -y python3-jinja2
+COPY ./ros-bridge-replacer.py /data/config/ros-bridge-replacer.py

Docker/duckietown/bridge.yml → Docker/duckietown/bridge.yml.j2

@@ -2,11 +2,11 @@ topics:
 
 # Camera
 - queue_size: 1
-  topic: /gazebo/camera_node_virtual/image
+  topic: /{{veh}}/virtual_camera_node/Image
   type: sensor_msgs/msg/Image
 
 - queue_size: 1
-  topic: /gazebo/camera_node_virtual/camera_info
+  topic: /{{veh}}/virtual_camera_node/camera_info
   type: sensor_msgs/msg/CameraInfo
 
 # Lidar

Docker/duckietown/gazebo-bridge-config.yaml → Docker/duckietown/gazebo-bridge-config.yaml.j2

@@ -4,18 +4,21 @@
 # Set each topic name explicitly
 
 # Virtual Camera
-- ros_topic_name: "/gazebo/camera_node_virtual/image"
+- ros_topic_name: "/{{veh}}/virtual_camera_node/Image"
   gz_topic_name: "/camera_front/Image"
   ros_type_name: "sensor_msgs/msg/Image"
   gz_type_name: "gz.msgs.Image"
+  direction: GZ_TO_ROS
 
-- ros_topic_name: "/gazebo/camera_node_virtual/camera_info"
+- ros_topic_name: "/{{veh}}/virtual_camera_node/CameraInfo"
   gz_topic_name: "/camera_front/CameraInfo"
   ros_type_name: "sensor_msgs/msg/CameraInfo"
   gz_type_name: "gz.msgs.CameraInfo"
+  direction: GZ_TO_ROS
 
 # Virtual Lidar (emulates ToF)
 - ros_topic_name: "/gazebo/lidar_node_virtual/scan"
   gz_topic_name: "/lidar"
   ros_type_name: "sensor_msgs/msg/LaserScan"
   gz_type_name: "gz.msgs.LaserScan"
+  direction: GZ_TO_ROS

Docker/duckietown/gazebo-bridge-replacer.py

@@ -0,0 +1,29 @@
+""" This script reads the `gazebo-bridge-config.yml` file and replaces the value of the `veh`
+key with the `VEH` environment variables. The string is of the format: 
+`$(env veh)/virtual_camera_node` where the `env` keys are taken from the shell environment. """
+
+import yaml
+import os
+import jinja2
+
+# Read the environment variables
+file_name = "gazebo-bridge-config.yaml"
+
+def substitute_env_variables(file_name):
+    env = jinja2.Environment(loader=jinja2.FileSystemLoader('/data/config'))
+    template = env.get_template(file_name + '.j2')
+    # Read the environment variables
+    substitution_variables = {'veh': os.getenv('VEH')}
+
+    env = jinja2.Environment(loader=jinja2.FileSystemLoader('/data/config'))
+    template = env.get_template(file_name + '.j2')
+    data = template.render(substitution_variables)
+
+    with open(f"/data/config/{file_name}", 'w') as stream:
+        try:
+            # Parse the data as YAML
+            yaml.dump(yaml.safe_load(data), stream)
+        except yaml.YAMLError as exc:
+            print(exc)
+
+substitute_env_variables("gazebo-bridge-config.yaml")

Docker/duckietown/ros-bridge-replacer.py

@@ -0,0 +1,27 @@
+""" This script reads the `bridge.yml` file and replaces the value of the `topic` key in the first two elements of the list with a string that uses environment variables. The string is of the format: `$(env veh)/$(env ROBOT_HARDWARE)_camera_node` where the `env` keys are taken from the shell environment. """
+
+import yaml
+import os
+import jinja2
+
+# Read the environment variables
+file_name = 'bridge.yml'
+
+def substitute_env_variables(file_name):
+    env = jinja2.Environment(loader=jinja2.FileSystemLoader('/data/config'))
+    template = env.get_template(file_name + '.j2')
+    # Read the environment variables
+    substitution_variables = {'veh': os.getenv('VEH')}
+
+    env = jinja2.Environment(loader=jinja2.FileSystemLoader('/data/config'))
+    template = env.get_template(file_name + '.j2')
+    data = template.render(substitution_variables)
+
+    with open(f"/data/config/{file_name}", 'w') as stream:
+        try:
+            # Parse the data as YAML
+            yaml.dump(yaml.safe_load(data), stream)
+        except yaml.YAMLError as exc:
+            print(exc)
+
+substitute_env_variables(file_name)

docker-compose.yml

@@ -27,20 +27,22 @@ services:
         context: ./Docker/duckietown
         dockerfile: Dockerfile.ros2-ros1-bridge
       environment:
-          - ROS_MASTER_URI=${ROS_MASTER_URI}
+          - VEH=${VEH}
+          - ROS_MASTER_URI=http://${VEH}.local:11311
           - ROS_IP=${ROS_IP}
           - FASTRTPS_DEFAULT_PROFILES_FILE=/data/config/fastdds_no_shared_memory.xml
       volumes:
         - /var/run/avahi-daemon/socket:/var/run/avahi-daemon/socket
       network_mode: host
-      command: ["bash", "-c", "source /ros_entrypoint.sh && rosparam load /data/config/bridge.yml && ros2 run ros1_bridge parameter_bridge"] 
+      command: ["bash", "-c", "source /ros_entrypoint.sh && cd /data/config && python3 ros-bridge-replacer.py && rosparam load /data/config/bridge.yml && ros2 run ros1_bridge parameter_bridge"] 
 
   gazebo-ros2-bridge:
       build:
         context: ./Docker/duckietown
         dockerfile: Dockerfile.gz-garden-bridge
       environment:
+          - VEH=${VEH}
           - FASTRTPS_DEFAULT_PROFILES_FILE=/data/config/fastdds_no_shared_memory.xml
 
       network_mode: host
-      command: ["bash", "-c", "source /opt/ros/humble/setup.bash && source /ws/install/setup.bash && ros2 run ros_gz_bridge parameter_bridge --ros-args -p config_file:=/data/config/gazebo-bridge-config.yaml"]
+      command: ["bash", "-c", "source /opt/ros/humble/setup.bash && python3 /data/config/gazebo-bridge-replacer.py && source /ws/install/setup.bash && ros2 run ros_gz_bridge parameter_bridge --ros-args -p config_file:=/data/config/gazebo-bridge-config.yaml"]