Multi robot support (#207)

* Working multi robot sim * Use wildcards for param files Added robot name to Create 3 buttons * Dock model is now called <robot_name>/standard_dock Namespaced dock frame id in pose republisher * Added namespacing support to Rviz config Rviz now launches for each robot spawn Added static transform for prefixed odom and base_link frames Added offset launch module in irobot_create_common_bringup Fixed spawning robots with different x, y, and yaw positions * Linter fixes * Added namespace launch arg. Defaults to 'robot_name' in create3_spawn create3_ignition defaults to spawning 'create3' robot without namespace * Frame id fixes * Changed robot name to be <namespace>/create3, rather than using a robot_name arg Changed dock name to be <namespace>/standard_dock Remove robot_name arg, use just namespace for everything * Fixed bumper Use ogre2 as sensor render engine Reduce max_step_size to 3ms * Linter fixes * Fixed import order * Multi robot support for gazebo classic * Updated README * Fixed import order
iRobotEducation · May 10, 2023 · 7fccefe · 7fccefe
1 parent f8ab281
commit 7fccefe
Show file tree

Hide file tree

Showing 60 changed files with 1,287 additions and 730 deletions.
diff --git a/README.md b/README.md
@@ -5,8 +5,6 @@
 This is a ROS 2 simulation stack for the [iRobot® Create® 3](https://edu.irobot.com/create3) robot.
 Both Ignition Gazebo and Classic Gazebo are supported.
 
-> :warning: **To run with Ignition Gazebo you must first build and install the [`gz_ros2_control`](https://github.com/ros-controls/gz_ros2_control) package master branch from sources!**
-
 Have a look at the [Create® 3 documentation](https://iroboteducation.github.io/create3_docs/) for more details on the ROS 2 interfaces exposed by the robot.
 
 ## Prerequisites
@@ -25,13 +23,13 @@ Besides the aforementioned dependencies you will also need at least one among Ig
 
 Install [Gazebo 11](http://gazebosim.org/tutorials?tut=install_ubuntu)
 
-#### Ignition Edifice
+#### Ignition Fortress
 
 ```bash
 sudo apt-get update && sudo apt-get install wget
 sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
 wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
-sudo apt-get update && sudo apt-get install ignition-edifice
+sudo apt-get update && sudo apt-get install ignition-fortress
 ```
 
 ## Build
@@ -55,7 +53,7 @@ rosdep install --from-path src -yi
 - Build the workspace with:
 
 ```bash
-export IGNITION_VERSION=edifice
+export IGNITION_VERSION=fortress
 colcon build --symlink-install
 source install/local_setup.bash
 ```
@@ -66,12 +64,35 @@ source install/local_setup.bash
 
 ##### Empty world
 
-Create® 3 can be spawned in an empty world in Gazebo and monitored through RViz with
+Create® 3 can be spawned in an empty world in Gazebo and monitored through RViz with:
 
 ```bash
 ros2 launch irobot_create_gazebo_bringup create3_gazebo.launch.py
 ```
 
+The spawn point can be changed with the `x`, `y`, `z` and `yaw` launch arguments:
+
+```bash
+ros2 launch irobot_create_gazebo_bringup create3_gazebo.launch.py x:=1.0 y:=0.5 yaw:=1.5707
+```
+
+##### Namespacing
+
+A namespace can be applied to the robot using the `namespace` launch argument:
+
+```bash
+ros2 launch irobot_create_gazebo_bringup create3_gazebo.launch.py namespace:=my_robot
+```
+
+Multiple robots can be spawned with unique namespaces:
+
+```bash
+ros2 launch irobot_create_gazebo_bringup create3_gazebo.launch.py namespace:=robot1
+ros2 launch irobot_create_gazebo_bringup create3_spawn.launch.py namespace:=robot2 x:=1.0
+```
+
+> :warning: `create3_gazebo.launch.py` should only be used once as it launches the Gazebo simulator itself. Additional robots should be spawned with `create3_spawn.launch.py`. Namespaces and spawn points should be unique for each robot.
+
 ##### AWS house
 
 Create® 3 can be spawned in the AWS small house in Gazebo and monitored through RViz.
@@ -99,6 +120,29 @@ Create® 3 can be spawned in a demo world in Ignition and monitored through RViz
 ros2 launch irobot_create_ignition_bringup create3_ignition.launch.py
 ```
 
+The spawn point can be changed with the `x`, `y`, `z` and `yaw` launch arguments:
+
+```bash
+ros2 launch irobot_create_ignition_bringup create3_ignition.launch.py x:=1.0 y:=0.5 yaw:=1.5707
+```
+
+##### Namespacing
+
+A namespace can be applied to the robot using the `namespace` launch argument:
+
+```bash
+ros2 launch irobot_create_ignition_bringup create3_ignition.launch.py namespace:=my_robot
+```
+
+Multiple robots can be spawned with unique namespaces:
+
+```bash
+ros2 launch irobot_create_ignition_bringup create3_ignition.launch.py namespace:=robot1
+ros2 launch irobot_create_ignition_bringup create3_spawn.launch.py namespace:=robot2 x:=1.0
+```
+
+> :warning: `create3_ignition.launch.py` should only be used once as it launches the Ignition simulator itself. Additional robots should be spawned with `create3_spawn.launch.py`. Namespaces and spawn points should be unique for each robot.
+
 ## Package layout
 
 This repository contains packages for both the Classic and Ignition Gazebo simulators:

diff --git a/irobot_create_common/irobot_create_common_bringup/CMakeLists.txt b/irobot_create_common/irobot_create_common_bringup/CMakeLists.txt
@@ -11,6 +11,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
 
 install(
   DIRECTORY
@@ -21,6 +22,9 @@ install(
     share/${PROJECT_NAME}
 )
 
+# Install Python modules
+ament_python_install_package(${PROJECT_NAME})
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()

diff --git a/irobot_create_common/irobot_create_common_bringup/config/hazard_vector_params.yaml b/irobot_create_common/irobot_create_common_bringup/config/hazard_vector_params.yaml
@@ -1,19 +1,20 @@
 ---
-hazards_vector_publisher:
-  ros__parameters:
-    # Hazard detection publisher topic
-    publisher_topic: /hazard_detection
-    publish_rate: 62.0
-    subscription_topics:
-      # Bumper topic
-      - _internal/bumper/event
-      # Cliff topics
-      - _internal/cliff_front_left/event
-      - _internal/cliff_front_right/event
-      - _internal/cliff_side_left/event
-      - _internal/cliff_side_right/event
-      # Wheel drop topics
-      - _internal/wheel_drop/left_wheel/event
-      - _internal/wheel_drop/right_wheel/event
-      # Backup limit topic
-      - _internal/backup_limit
+/**:
+  hazards_vector_publisher:
+    ros__parameters:
+      # Hazard detection publisher topic
+      publisher_topic: hazard_detection
+      publish_rate: 62.0
+      subscription_topics:
+        # Bumper topic
+        - _internal/bumper/event
+        # Cliff topics
+        - _internal/cliff_front_left/event
+        - _internal/cliff_front_right/event
+        - _internal/cliff_side_left/event
+        - _internal/cliff_side_right/event
+        # Wheel drop topics
+        - _internal/wheel_drop/left_wheel/event
+        - _internal/wheel_drop/right_wheel/event
+        # Backup limit topic
+        - _internal/backup_limit
diff --git a/irobot_create_common/irobot_create_common_bringup/config/ir_intensity_vector_params.yaml b/irobot_create_common/irobot_create_common_bringup/config/ir_intensity_vector_params.yaml
@@ -1,15 +1,16 @@
 ---
-ir_intensity_vector_publisher:
-  ros__parameters:
-    # IR intensity publisher topic
-    publisher_topic: /ir_intensity
-    publish_rate: 62.0
-    subscription_topics:
-      # IR intensity topics
-      - _internal/ir_intensity_front_center_left
-      - _internal/ir_intensity_front_center_right
-      - _internal/ir_intensity_front_left
-      - _internal/ir_intensity_front_right
-      - _internal/ir_intensity_left
-      - _internal/ir_intensity_right
-      - _internal/ir_intensity_side_left
+/**:
+  ir_intensity_vector_publisher:
+    ros__parameters:
+      # IR intensity publisher topic
+      publisher_topic: ir_intensity
+      publish_rate: 62.0
+      subscription_topics:
+        # IR intensity topics
+        - _internal/ir_intensity_front_center_left
+        - _internal/ir_intensity_front_center_right
+        - _internal/ir_intensity_front_left
+        - _internal/ir_intensity_front_right
+        - _internal/ir_intensity_left
+        - _internal/ir_intensity_right
+        - _internal/ir_intensity_side_left
diff --git a/irobot_create_common/irobot_create_common_bringup/config/kidnap_estimator_params.yaml b/irobot_create_common/irobot_create_common_bringup/config/kidnap_estimator_params.yaml
@@ -1,7 +1,8 @@
 ---
-kidnap_estimator_publisher:
-  ros__parameters:
-    # Kidnap status publisher topic
-    kidnap_status_topic: /kidnap_status
-    # Subscription topics
-    hazard_topic: /hazard_detection
+/**:
+  kidnap_estimator_publisher:
+    ros__parameters:
+      # Kidnap status publisher topic
+      kidnap_status_topic: kidnap_status
+      # Subscription topics
+      hazard_topic: hazard_detection
diff --git a/irobot_create_common/irobot_create_common_bringup/config/mock_params.yaml b/irobot_create_common/irobot_create_common_bringup/config/mock_params.yaml
@@ -1,7 +1,8 @@
 ---
-mock_publisher:
-  ros__parameters:
-    # Mock slip status publisher topic
-    slip_status_topic: /slip_status
-    # Publishers rate
-    slip_status_publish_rate: 62.0
+/**:
+  mock_publisher:
+    ros__parameters:
+      # Mock slip status publisher topic
+      slip_status_topic: slip_status
+      # Publishers rate
+      slip_status_publish_rate: 62.0
diff --git a/irobot_create_common/irobot_create_common_bringup/config/robot_state_params.yaml b/irobot_create_common/irobot_create_common_bringup/config/robot_state_params.yaml
@@ -1,22 +1,23 @@
 ---
-robot_state:
-  ros__parameters:
-    # Stop status publisher topic
-    stop_status_topic: /stop_status
-    # Mock battery state publisher topic
-    battery_state_topic: /battery_state
-    # Publishers rate
-    kidnap_status_publish_rate: 1.0
-    stop_status_publish_rate: 62.0
-    battery_state_publish_rate: 0.1
-    # Subscription topics
-    dock_topic: /dock_status
-    wheel_vels_topic: /odom
-    # Stop status position difference tolerance
-    linear_velocity_tolerance: 0.01
-    angular_velocity_tolerance: 0.1
-    # Battery parameters
-    full_charge_percentage: 1.0
-    battery_high_percentage: 0.9
-    # Dock Parameters
-    undocked_charge_limit: 0.03
+/**:
+  robot_state:
+    ros__parameters:
+      # Stop status publisher topic
+      stop_status_topic: stop_status
+      # Mock battery state publisher topic
+      battery_state_topic: battery_state
+      # Publishers rate
+      kidnap_status_publish_rate: 1.0
+      stop_status_publish_rate: 62.0
+      battery_state_publish_rate: 0.1
+      # Subscription topics
+      dock_topic: dock_status
+      wheel_vels_topic: odom
+      # Stop status position difference tolerance
+      linear_velocity_tolerance: 0.01
+      angular_velocity_tolerance: 0.1
+      # Battery parameters
+      full_charge_percentage: 1.0
+      battery_high_percentage: 0.9
+      # Dock Parameters
+      undocked_charge_limit: 0.03
diff --git a/irobot_create_common/irobot_create_common_bringup/config/ui_mgr_params.yaml b/irobot_create_common/irobot_create_common_bringup/config/ui_mgr_params.yaml
@@ -1,10 +1,11 @@
 ---
-ui_mgr:
-  ros__parameters:
-    # Buttons publisher topic
-    button_topic: /interface_buttons
-    # Publishers rate
-    buttons_publish_rate: 1.0
-    # Subscription topics
-    lightring_topic: /cmd_lightring
-    audio_topic: /cmd_audio
+/**:
+  ui_mgr:
+    ros__parameters:
+      # Buttons publisher topic
+      button_topic: interface_buttons
+      # Publishers rate
+      buttons_publish_rate: 1.0
+      # Subscription topics
+      lightring_topic: cmd_lightring
+      audio_topic: cmd_audio
diff --git a/irobot_create_common/irobot_create_common_bringup/config/wheel_status_params.yaml b/irobot_create_common/irobot_create_common_bringup/config/wheel_status_params.yaml
@@ -1,13 +1,14 @@
 ---
-wheel_status_publisher:
-  ros__parameters:
-    # Publish rate
-    publish_rate: 62.0
-    # Encoder resolution
-    encoder_resolution: 508.8
-    # Wheel radius
-    wheel_radius: 0.03575
-    # Wheels angular velocity topic
-    velocity_topic: /wheel_vels
-    # Wheels' net encoder ticks topic
-    ticks_topic: /wheel_ticks
+/**:
+  wheel_status_publisher:
+    ros__parameters:
+      # Publish rate
+      publish_rate: 62.0
+      # Encoder resolution
+      encoder_resolution: 508.8
+      # Wheel radius
+      wheel_radius: 0.03575
+      # Wheels angular velocity topic
+      velocity_topic: wheel_vels
+      # Wheels' net encoder ticks topic
+      ticks_topic: wheel_ticks
diff --git a/irobot_create_common/irobot_create_common_bringup/irobot_create_common_bringup/__init__.py b/irobot_create_common/irobot_create_common_bringup/irobot_create_common_bringup/__init__.py
diff --git a/irobot_create_common/irobot_create_common_bringup/irobot_create_common_bringup/namespace.py b/irobot_create_common/irobot_create_common_bringup/irobot_create_common_bringup/namespace.py
@@ -0,0 +1,33 @@
+from typing import Union
+
+from launch import LaunchContext, SomeSubstitutionsType, Substitution
+
+
+class GetNamespacedName(Substitution):
+    def __init__(
+            self,
+            namespace: Union[SomeSubstitutionsType, str],
+            name: Union[SomeSubstitutionsType, str]
+    ) -> None:
+        self.__namespace = namespace
+        self.__name = name
+
+    def perform(
+            self,
+            context: LaunchContext = None,
+    ) -> str:
+        if isinstance(self.__namespace, Substitution):
+            namespace = str(self.__namespace.perform(context))
+        else:
+            namespace = str(self.__namespace)
+
+        if isinstance(self.__name, Substitution):
+            name = str(self.__name.perform(context))
+        else:
+            name = str(self.__name)
+
+        if namespace == '':
+            namespaced_name = name
+        else:
+            namespaced_name = namespace + '/' + name
+        return namespaced_name