Added namespace to bringup

Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com>

rosbot_xl_bringup/config/ekf.yaml

@@ -1,7 +1,7 @@
 # Ref: http://docs.ros.org/en/noetic/api/robot_localization/html/state_estimation_nodes.html
 
 ## ekf config file ###
-ekf_filter_node:
+/**/ekf_filter_node:
     ros__parameters:
         frequency: 25.0
         sensor_timeout: 0.05
@@ -17,7 +17,7 @@ ekf_filter_node:
         publish_tf: true
         publish_acceleration: false
 
-        odom0: /rosbot_xl_base_controller/odom
+        odom0: rosbot_xl_base_controller/odom
         odom0_config: [false, false, false, # X , Y , Z
                       false, false, false, # roll , pitch ,yaw
                       true, true, false, # dX , dY , dZ
@@ -29,7 +29,7 @@ ekf_filter_node:
         odom0_differential: false
         odom0_relative: true
 
-        imu0: /imu_broadcaster/imu
+        imu0: imu_broadcaster/imu
         imu0_config: [false, false, false, # X , Y , Z
                       false, false, true, # roll , pitch ,yaw
                       false, false, false, # dX , dY , dZ

rosbot_xl_bringup/config/laser_filter.yaml

@@ -1,10 +1,11 @@
-scan_to_scan_filter_chain:
+/**/scan_to_scan_filter_chain:
   ros__parameters:
     filter1:
       name: box_filter
       type: laser_filters/LaserScanBoxFilter
       params:
-        box_frame: base_link
+        # This parameter is replaced in the launch file
+        # box_frame: base_link
 
         max_x: 0.165
         min_x: -0.195 # 0.165 + 0.03 (wifi antenna)

rosbot_xl_bringup/launch/bringup.launch.py

@@ -14,10 +14,7 @@
 
 from launch import LaunchDescription
 from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
-from launch.substitutions import (
-    PathJoinSubstitution,
-    LaunchConfiguration,
-)
+from launch.substitutions import PathJoinSubstitution, LaunchConfiguration, PythonExpression
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 
 from launch_ros.actions import Node, SetParameter
@@ -26,6 +23,16 @@
 
 
 def generate_launch_description():
+    namespace = LaunchConfiguration("namespace")
+    namespace_tf_prefix = PythonExpression([
+        "''", " if '", namespace, "' == '' ", "else ", "'", namespace, "_'"
+    ])
+    declare_namespace_arg = DeclareLaunchArgument(
+        "namespace",
+        default_value="",
+        description="Namespace for all topics and tfs",
+    )
+
     mecanum = LaunchConfiguration("mecanum")
     declare_mecanum_arg = DeclareLaunchArgument(
         "mecanum",
@@ -88,10 +95,13 @@ def generate_launch_description():
         description="Which simulation engine will be used",
     )
 
+    rosbot_xl_controller = get_package_share_directory("rosbot_xl_controller")
+    rosbot_xl_bringup = get_package_share_directory("rosbot_xl_bringup")
+
     controller_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             PathJoinSubstitution([
-                get_package_share_directory("rosbot_xl_controller"),
+                rosbot_xl_controller,
                 "launch",
                 "controller.launch.py",
             ])
@@ -103,34 +113,65 @@ def generate_launch_description():
             "include_camera_mount": include_camera_mount,
             "use_sim": use_sim,
             "simulation_engine": simulation_engine,
+            "namespace": namespace,
         }.items(),
     )
 
+    ekf_config = PathJoinSubstitution([rosbot_xl_bringup, "config", "ekf.yaml"])
+
     robot_localization_node = Node(
         package="robot_localization",
         executable="ekf_node",
         name="ekf_filter_node",
         output="screen",
         parameters=[
-            PathJoinSubstitution([
-                get_package_share_directory("rosbot_xl_bringup"), "config", "ekf.yaml"
-            ])
+            ekf_config,
+            {
+                "map_frame": LaunchConfiguration(
+                    "ekf_map_frame", default=[namespace_tf_prefix, "map"]
+                )
+            },
+            {
+                "odom_frame": LaunchConfiguration(
+                    "ekf_odom_frame", default=[namespace_tf_prefix, "odom"]
+                )
+            },
+            {
+                "base_link_frame": LaunchConfiguration(
+                    "ekf_base_link_frame", default=[namespace_tf_prefix, "base_link"]
+                )
+            },
+            {
+                "world_frame": LaunchConfiguration(
+                    "ekf_world_frame", default=[namespace_tf_prefix, "odom"]
+                )
+            },
         ],
+        namespace=namespace,
     )
 
+    laser_filter_config = PathJoinSubstitution([
+        rosbot_xl_bringup,
+        "config",
+        "laser_filter.yaml",
+    ])
+
     laser_filter_node = Node(
         package="laser_filters",
         executable="scan_to_scan_filter_chain",
         parameters=[
-            PathJoinSubstitution([
-                get_package_share_directory("rosbot_xl_bringup"),
-                "config",
-                "laser_filter.yaml",
-            ])
+            laser_filter_config,
+            {
+                "filter1.params.box_frame": LaunchConfiguration(
+                    "laser_filter_box_frame", default=[namespace_tf_prefix, "base_link"]
+                )
+            },
         ],
+        namespace=namespace,
     )
 
-    actions = [
+    return LaunchDescription([
+        declare_namespace_arg,
         declare_mecanum_arg,
         declare_lidar_model_arg,
         declare_camera_model_arg,
@@ -141,6 +182,4 @@ def generate_launch_description():
         controller_launch,
         robot_localization_node,
         laser_filter_node,
-    ]
-
-    return LaunchDescription(actions)
+    ])

rosbot_xl_bringup/test/test_diff_drive_ekf_and_scan.py

@@ -41,6 +41,7 @@ def generate_test_description():
             "use_sim": "False",
             "mecanum": "False",
             "use_gpu": "False",
+            "namespace": "rosbotxl",
         }.items(),
     )
 

rosbot_xl_bringup/test/test_mecanum_ekf_and_scan.py

@@ -41,6 +41,7 @@ def generate_test_description():
             "use_sim": "False",
             "mecanum": "True",
             "use_gpu": "False",
+            "namespace": "rosbotxl",
         }.items(),
     )
 

rosbot_xl_bringup/test/test_utils.py

@@ -38,23 +38,25 @@ def __init__(self, name="test_node"):
         self.scan_filter_event = Event()
 
     def create_test_subscribers_and_publishers(self):
-        self.imu_publisher = self.create_publisher(Imu, "_imu/data_raw", 10)
+        self.imu_publisher = self.create_publisher(Imu, "/_imu/data_raw", 10)
 
-        self.joint_states_publisher = self.create_publisher(JointState, "_motors_response", 10)
+        self.joint_states_publisher = self.create_publisher(JointState, "/_motors_response", 10)
 
         self.tf_buffer = Buffer()
         self.tf_listener = TransformListener(self.tf_buffer, self)
 
-        self.scan_publisher = self.create_publisher(LaserScan, "scan", 10)
+        self.scan_publisher = self.create_publisher(LaserScan, "/rosbotxl/scan", 10)
         self.filtered_scan_subscriber = self.create_subscription(
-            LaserScan, "scan_filtered", self.filtered_scan_callback, 10
+            LaserScan, "/rosbotxl/scan_filtered", self.filtered_scan_callback, 10
         )
 
         self.timer = None
 
     def lookup_transform_odom(self):
         try:
-            self.tf_buffer.lookup_transform("odom", "base_link", rclpy.time.Time())
+            self.tf_buffer.lookup_transform(
+                "rosbotxl_odom", "rosbotxl_base_link", rclpy.time.Time()
+            )
             self.odom_tf_event.set()
         except TransformException as ex:
             self.get_logger().error(f"Could not transform odom to base_link: {ex}")
@@ -99,7 +101,7 @@ def publish_fake_hardware_messages(self):
 
     def publish_scan(self):
         msg = LaserScan()
-        msg.header.frame_id = "base_link"
+        msg.header.frame_id = "rosbotxl_base_link"
         msg.angle_min = 0.0
         msg.angle_max = 2.0 * math.pi
         msg.angle_increment = 0.05