added namespaces to ros2_control

Signed-off-by: Jakub Delicat <[email protected]>

rosbot_controller/config/diff_drive_controller.yaml

@@ -4,7 +4,7 @@ simulation_ignition_ros_control:
 
 # Separate controller manager used for simulation - only difference is
 # the use_sim_time parameter (it is the easiest way to do it with ign ros2 control)
-simulation_controller_manager:
+/**/simulation_controller_manager:
   ros__parameters:
     use_sim_time: true
     update_rate: 20 # Hz
@@ -17,7 +17,7 @@ simulation_controller_manager:
       type: imu_sensor_broadcaster/IMUSensorBroadcaster
 
 
-controller_manager:
+/**/controller_manager:
   ros__parameters:
     use_sim_time: false
     update_rate: 20 # Hz
@@ -29,15 +29,15 @@ controller_manager:
     imu_broadcaster:
       type: imu_sensor_broadcaster/IMUSensorBroadcaster
 
-imu_broadcaster:
+/**/imu_broadcaster:
   ros__parameters:
     sensor_name: imu
     frame_id: imu_link
     static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet
     static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values measured experimentally
     static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0, 0.0] # Values measured experimentally
 
-rosbot_base_controller:
+/**/rosbot_base_controller:
   ros__parameters:
     left_wheel_names: ["fl_wheel_joint", "rl_wheel_joint"]
     right_wheel_names: ["fr_wheel_joint", "rr_wheel_joint"]

rosbot_controller/launch/controller.launch.py

@@ -14,7 +14,7 @@
 # limitations under the License.
 
 from launch import LaunchDescription
-from launch.actions import RegisterEventHandler, DeclareLaunchArgument
+from launch.actions import RegisterEventHandler, DeclareLaunchArgument, GroupAction
 from launch.conditions import UnlessCondition
 from launch.event_handlers import OnProcessExit
 from launch.substitutions import (
@@ -30,6 +30,13 @@
 
 
 def generate_launch_description():
+    namespace = LaunchConfiguration("namespace")
+    declare_namespace_arg = DeclareLaunchArgument(
+        "namespace",
+        default_value="",
+        description="Namespace for all topics and tfs",
+    )
+
     mecanum = LaunchConfiguration("mecanum")
     declare_mecanum_arg = DeclareLaunchArgument(
         "mecanum",
@@ -69,13 +76,24 @@ def generate_launch_description():
         ]
     )
 
-    controller_manager_name = PythonExpression(
+    controller_manager_type_name = PythonExpression(
         [
-            "'/simulation_controller_manager' if ",
+            "'simulation_controller_manager' if ",
             use_sim,
-            " else '/controller_manager'",
+            " else 'controller_manager'",
         ]
     )
+    controller_manager_name = LaunchConfiguration("controller_manager_name")
+    namespace_for_controller_name = PythonExpression(
+        ["''", " if '", namespace, "' == '' ", "else ", "'", namespace, "/'"]
+    )
+
+    namespace_for_controller_name
+    declare_controller_manager_name_arg = DeclareLaunchArgument(
+        "controller_manager_name",
+        default_value=[namespace_for_controller_name, controller_manager_type_name],
+        description="ros2_control controller manager name",
+    )
 
     # Get URDF via xacro
     robot_description_content = Command(
@@ -114,12 +132,13 @@ def generate_launch_description():
         executable="ros2_control_node",
         parameters=[robot_description, robot_controllers],
         remappings=[
-            ("/imu_sensor_node/imu", "/_imu/data_raw"),
+            ("imu_sensor_node/imu", "/_imu/data_raw"),
             ("~/motors_cmd", "/_motors_cmd"),
             ("~/motors_response", "/_motors_response"),
-            ("/rosbot_base_controller/cmd_vel_unstamped", "/cmd_vel"),
+            ("rosbot_base_controller/cmd_vel_unstamped", "cmd_vel"),
         ],
         condition=UnlessCondition(use_sim),
+        namespace=namespace,
     )
 
     robot_state_pub_node = Node(
@@ -137,6 +156,8 @@ def generate_launch_description():
             controller_manager_name,
             "--controller-manager-timeout",
             "120",
+            "--namespace",
+            namespace,
         ],
     )
 
@@ -149,6 +170,8 @@ def generate_launch_description():
             controller_manager_name,
             "--controller-manager-timeout",
             "120",
+            "--namespace",
+            namespace,
         ],
     )
 
@@ -169,6 +192,8 @@ def generate_launch_description():
             controller_manager_name,
             "--controller-manager-timeout",
             "120",
+            "--namespace",
+            namespace,
         ],
     )
 
@@ -181,17 +206,32 @@ def generate_launch_description():
         )
     )
 
-    actions = [
+    args_declarations_actions = [
+        declare_namespace_arg,
         declare_mecanum_arg,
         declare_use_sim_arg,
         declare_use_gpu_arg,
         declare_simulation_engine_arg,
-        SetParameter(name="use_sim_time", value=use_sim),
+        declare_controller_manager_name_arg,
+    ]
+
+    nodes_actions = [
         control_node,
         robot_state_pub_node,
         joint_state_broadcaster_spawner,
         delay_robot_controller_spawner_after_joint_state_broadcaster_spawner,
         delay_imu_broadcaster_spawner_after_robot_controller_spawner,
     ]
 
-    return LaunchDescription(actions)
+    return LaunchDescription(
+        args_declarations_actions
+        + [
+            GroupAction(
+                actions=[
+                    # PushRosNamespace(namespace),
+                    SetParameter(name="use_sim_time", value=use_sim),
+                ]
+                + nodes_actions
+            )
+        ]
+    )