diff --git a/launch/localization.launch.py b/launch/localization.launch.py index 54106e4..8477cc1 100644 --- a/launch/localization.launch.py +++ b/launch/localization.launch.py @@ -48,6 +48,8 @@ from launch_ros.actions import PushRosNamespace +from nav2_common.launch import RewrittenYaml + ARGUMENTS = [ DeclareLaunchArgument('use_sim_time', default_value='false', @@ -55,7 +57,10 @@ 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) ] ), ]) diff --git a/launch/nav2.launch.py b/launch/nav2.launch.py index d3ca4bb..7d45d38 100644 --- a/launch/nav2.launch.py +++ b/launch/nav2.launch.py @@ -48,6 +48,8 @@ from launch_ros.actions import PushRosNamespace, SetRemap +from nav2_common.launch import RewrittenYaml + ARGUMENTS = [ DeclareLaunchArgument('use_sim_time', default_value='false', @@ -55,7 +57,10 @@ 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,21 +82,32 @@ 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'), @@ -98,7 +115,7 @@ def launch_setup(context, *args, **kwargs): PythonLaunchDescriptionSource(launch_nav2), launch_arguments=[ ('use_sim_time', use_sim_time), - ('params_file', file_parameters), + ('params_file', rewritten_parameters), ('use_composition', 'False'), ('namespace', namespace) ] diff --git a/launch/slam.launch.py b/launch/slam.launch.py index ab6d363..f578016 100644 --- a/launch/slam.launch.py +++ b/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 )