Add scan_topic launch argument (#32)

* Add `scan_topic` launch argument for slam, nav2 localization launch files

Author: civerachb-cpr

launch/localization.launch.py

@@ -48,14 +48,19 @@
 
 from launch_ros.actions import PushRosNamespace
 
+from nav2_common.launch import RewrittenYaml
+
 
 ARGUMENTS = [
     DeclareLaunchArgument('use_sim_time', default_value='false',
                           choices=['true', 'false'],
                           description='Use sim time'),
     DeclareLaunchArgument('setup_path',
                           default_value='/etc/clearpath/',
-                          description='Clearpath setup path')
+                          description='Clearpath setup path'),
+    DeclareLaunchArgument('scan_topic',
+                          default_value='',
+                          description='Override the default 2D laserscan topic')
 ]
 
 
@@ -68,6 +73,7 @@ def launch_setup(context, *args, **kwargs):
     use_sim_time = LaunchConfiguration('use_sim_time')
     setup_path = LaunchConfiguration('setup_path')
     map = LaunchConfiguration('map')  # noqa:A001
+    scan_topic = LaunchConfiguration('scan_topic')
 
     # Read robot YAML
     config = read_yaml(os.path.join(setup_path.perform(context), 'robot.yaml'))
@@ -77,12 +83,24 @@ def launch_setup(context, *args, **kwargs):
     namespace = clearpath_config.system.namespace
     platform_model = clearpath_config.platform.get_platform_model()
 
+    eval_scan_topic = scan_topic.perform(context)
+    if len(eval_scan_topic) == 0:
+        eval_scan_topic = f'/{namespace}/sensors/lidar2d_0/scan'
+
     file_parameters = PathJoinSubstitution([
         pkg_clearpath_nav2_demos,
         'config',
         platform_model,
         'localization.yaml'])
 
+    rewritten_parameters = RewrittenYaml(
+        source_file=file_parameters,
+        param_rewrites={
+            'scan_topic': eval_scan_topic,
+        },
+        convert_types=True
+    )
+
     launch_localization = PathJoinSubstitution(
       [pkg_nav2_bringup, 'launch', 'localization_launch.py'])
 
@@ -95,7 +113,7 @@ def launch_setup(context, *args, **kwargs):
                 ('namespace', namespace),
                 ('map', map),
                 ('use_sim_time', use_sim_time),
-                ('params_file', file_parameters)
+                ('params_file', rewritten_parameters)
               ]
         ),
     ])

launch/nav2.launch.py

@@ -48,14 +48,19 @@
 
 from launch_ros.actions import PushRosNamespace, SetRemap
 
+from nav2_common.launch import RewrittenYaml
+
 
 ARGUMENTS = [
     DeclareLaunchArgument('use_sim_time', default_value='false',
                           choices=['true', 'false'],
                           description='Use sim time'),
     DeclareLaunchArgument('setup_path',
                           default_value='/etc/clearpath/',
-                          description='Clearpath setup path')
+                          description='Clearpath setup path'),
+    DeclareLaunchArgument('scan_topic',
+                          default_value='',
+                          description='Override the default 2D laserscan topic')
 ]
 
 
@@ -67,6 +72,7 @@ def launch_setup(context, *args, **kwargs):
     # Launch Configurations
     use_sim_time = LaunchConfiguration('use_sim_time')
     setup_path = LaunchConfiguration('setup_path')
+    scan_topic = LaunchConfiguration('scan_topic')
 
     # Read robot YAML
     config = read_yaml(os.path.join(setup_path.perform(context), 'robot.yaml'))
@@ -76,29 +82,40 @@ def launch_setup(context, *args, **kwargs):
     namespace = clearpath_config.system.namespace
     platform_model = clearpath_config.platform.get_platform_model()
 
+    # see if we've overridden the scan_topic
+    eval_scan_topic = scan_topic.perform(context)
+    if len(eval_scan_topic) == 0:
+        eval_scan_topic = f'/{namespace}/sensors/lidar2d_0/scan'
+
     file_parameters = PathJoinSubstitution([
         pkg_clearpath_nav2_demos,
         'config',
         platform_model,
         'nav2.yaml'])
 
+    rewritten_parameters = RewrittenYaml(
+        source_file=file_parameters,
+        param_rewrites={
+            # the only *.topic parameters are scan.topic, so rewrite all of them to point to
+            # our desired scan_topic
+            'topic': eval_scan_topic,
+        },
+        convert_types=True
+    )
+
     launch_nav2 = PathJoinSubstitution(
       [pkg_nav2_bringup, 'launch', 'navigation_launch.py'])
 
     nav2 = GroupAction([
         PushRosNamespace(namespace),
-        SetRemap('/' + namespace + '/global_costmap/sensors/lidar2d_0/scan',
-                 '/' + namespace + '/sensors/lidar2d_0/scan'),
-        SetRemap('/' + namespace + '/local_costmap/sensors/lidar2d_0/scan',
-                 '/' + namespace + '/sensors/lidar2d_0/scan'),
         SetRemap('/' + namespace + '/odom',
                  '/' + namespace + '/platform/odom'),
 
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(launch_nav2),
             launch_arguments=[
                 ('use_sim_time', use_sim_time),
-                ('params_file', file_parameters),
+                ('params_file', rewritten_parameters),
                 ('use_composition', 'False'),
                 ('namespace', namespace)
               ]

launch/slam.launch.py

@@ -67,6 +67,9 @@
     DeclareLaunchArgument('sync', default_value='true',
                           choices=['true', 'false'],
                           description='Use synchronous SLAM'),
+    DeclareLaunchArgument('scan_topic',
+                          default_value='',
+                          description='Override the default 2D laserscan topic')
 ]
 
 
@@ -81,6 +84,7 @@ def launch_setup(context, *args, **kwargs):
     autostart = LaunchConfiguration('autostart')
     use_lifecycle_manager = LaunchConfiguration('use_lifecycle_manager')
     sync = LaunchConfiguration('sync')
+    scan_topic = LaunchConfiguration('scan_topic')
 
     # Read robot YAML
     config = read_yaml(os.path.join(setup_path.perform(context), 'robot.yaml'))
@@ -89,6 +93,10 @@ def launch_setup(context, *args, **kwargs):
 
     namespace = clearpath_config.system.namespace
     platform_model = clearpath_config.platform.get_platform_model()
+    eval_scan_topic = scan_topic.perform(context)
+
+    if len(eval_scan_topic) == 0:
+        eval_scan_topic = f'/{namespace}/sensors/lidar2d_0/scan'
 
     file_parameters = PathJoinSubstitution([
         pkg_clearpath_nav2_demos,
@@ -101,7 +109,7 @@ def launch_setup(context, *args, **kwargs):
         root_key=namespace,
         param_rewrites={
             'map_name': '/' + namespace + '/map',
-            'scan_topic': '/' + namespace + '/sensors/lidar2d_0/scan',
+            'scan_topic': eval_scan_topic,
         },
         convert_types=True
     )