From b5d62dddadb8126b8d9bcf5a4f5cf2c4ebaa0b1e Mon Sep 17 00:00:00 2001 From: Jose Antonio Mendez Date: Tue, 23 Jun 2026 10:15:10 +0200 Subject: [PATCH] Modify launch and config for multirobot simulation support. Add robot_complete launch. --- .../launch/bringup_complete.launch.py | 124 ++------- .../launch/robot_complete.launch.py | 242 ++++++++++++++++++ .../config/amcl.yaml | 8 +- .../config/slam_toolbox.yaml | 8 +- .../launch/localization_2d.launch.py | 15 +- .../launch/mapping_2d.launch.py | 12 +- .../config/behavior_server.yaml | 6 +- .../config/bt_navigator.yaml | 4 +- .../config/controller_server.yaml | 24 +- .../config/planner_server.yaml | 6 +- .../config/route_server.yaml | 4 +- .../config/smoother_server.yaml | 2 +- .../launch/nav2_mission.launch.py | 23 +- .../launch/nav2_task.launch.py | 56 ++-- 14 files changed, 367 insertions(+), 167 deletions(-) create mode 100644 common/robotnik_simulation_bringup/launch/robot_complete.launch.py diff --git a/common/robotnik_simulation_bringup/launch/bringup_complete.launch.py b/common/robotnik_simulation_bringup/launch/bringup_complete.launch.py index 055154e..4455bf1 100644 --- a/common/robotnik_simulation_bringup/launch/bringup_complete.launch.py +++ b/common/robotnik_simulation_bringup/launch/bringup_complete.launch.py @@ -78,6 +78,16 @@ def generate_launch_description(): default_value="false", description="Enable MoveIt for manipulation" ), + DeclareLaunchArgument( + "run_localization", + default_value="true", + description="Enable localization stack" + ), + DeclareLaunchArgument( + "run_navigation", + default_value="true", + description="Enable navigation stack" + ), DeclareLaunchArgument( "arm_type", default_value="ur10e", @@ -102,6 +112,8 @@ def generate_launch_description(): world_path = LaunchConfiguration("world_path") use_rviz = LaunchConfiguration("use_rviz") run_moveit = LaunchConfiguration("run_moveit") + run_localization = LaunchConfiguration("run_localization") + run_navigation = LaunchConfiguration("run_navigation") arm_type = LaunchConfiguration("arm_type") gazebo_world = IncludeLaunchDescription( @@ -117,10 +129,10 @@ def generate_launch_description(): }.items() ) - gazebo_robot = IncludeLaunchDescription( + robot_complete = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution([ - FindPackageShare('robotnik_gazebo_ignition'), 'launch/spawn_robot.launch.py' + FindPackageShare('robotnik_simulation_bringup'), 'launch/robot_complete.launch.py' ]) ), launch_arguments={ @@ -128,116 +140,18 @@ def generate_launch_description(): 'robot': robot, 'robot_model': robot_model, 'robot_xacro_path': robot_xacro_path, + 'run_moveit': run_moveit, + 'run_localization': run_localization, + 'run_navigation': run_navigation, 'arm_type': arm_type, 'low_performance_simulation': low_performance_simulation, - 'run_rviz': 'false' - }.items() - ) - - # In spawn_robot.launch.py the add_laser("front") is defined for any robot model - # This causes two publishers to /robot/front_laser/scan when laser filters are enabled - # In rbsummit and rbwatcher is not an issue because front laser is not available - # but in other robot models that have front laser, it causes conflict. - laser_filters = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution([ - FindPackageShare('robotnik_simulation_bringup'), 'launch/laser_filters.launch.py' - ]) - ), - launch_arguments={ - 'robot_id': robot_id, - 'use_sim': 'true', - }.items(), - condition=IfCondition( - OrSubstitution( - OrSubstitution( - EqualsSubstitution(robot_model, 'rbsummit'), - EqualsSubstitution(robot_model, 'rbwatcher'), - ), - EqualsSubstitution(robot_model, 'rbcar'), - ) - ) - ) - - localization = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution([ - FindPackageShare('robotnik_simulation_localization'), 'launch/localization.launch.py' - ]) - ), - launch_arguments={ - 'robot_id': robot_id, - 'use_sim': 'true', - }.items() - ) - - delayed_localization = TimerAction( - period=10.0, - actions=[localization] - ) - - navigation = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution([ - FindPackageShare('robotnik_simulation_navigation'), 'launch/navigation.launch.py' - ]) - ), - launch_arguments={ - 'robot_id': robot_id, - 'use_sim': 'true', + 'use_rviz': use_rviz, }.items() ) - delayed_navigation = TimerAction( - period=15.0, - actions=[navigation] - ) - - rviz = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution([ - FindPackageShare('robotnik_simulation_bringup'), 'launch/rviz.launch.py' - ]) - ), - condition=IfCondition(use_rviz) - ) - - delayed_rviz = TimerAction( - period=20.0, - actions=[rviz] - ) - - moveit = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution([ - FindPackageShare('robotnik_simulation_moveit'), 'launch/moveit.launch.py', - ]) - ), - launch_arguments={ - 'robot_id': robot_id, - 'robot': robot, - 'robot_model': robot_model, - 'robot_xacro_path': robot_xacro_path, - 'moveit_config_name': [robot, '_moveit_config'], - 'arm_type': arm_type, - 'use_sim_time': 'true', - }.items(), - condition=IfCondition(run_moveit), - ) - - delayed_moveit = TimerAction( - period=25.0, - actions=[moveit] - ) - group = GroupAction([ gazebo_world, - gazebo_robot, - laser_filters, - delayed_localization, - delayed_navigation, - delayed_rviz, - delayed_moveit, + robot_complete ]) return LaunchDescription(declared_arguments + [group]) \ No newline at end of file diff --git a/common/robotnik_simulation_bringup/launch/robot_complete.launch.py b/common/robotnik_simulation_bringup/launch/robot_complete.launch.py new file mode 100644 index 0000000..f2ce442 --- /dev/null +++ b/common/robotnik_simulation_bringup/launch/robot_complete.launch.py @@ -0,0 +1,242 @@ +# Copyright (c) 2025, Robotnik Automation S.L.L. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# * Neither the name of the Robotnik Automation S.L.L. nor the +# names of its contributors may be used to endorse or promote products +# derived from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL Robotnik Automation S.L.L. BE LIABLE FOR ANY +# DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +from launch import LaunchDescription +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import GroupAction, DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.actions import TimerAction +from launch.substitutions import EqualsSubstitution, OrSubstitution + +def generate_launch_description(): + declared_arguments = [ + DeclareLaunchArgument( + "robot_id", + default_value="robot", + description="Name for launch and config resources" + ), + DeclareLaunchArgument( + "robot", + default_value="rbsummit", + description="Robot Model Name" + ), + DeclareLaunchArgument( + "robot_model", + default_value="rbsummit", + description="Robot Variant or Type" + ), + DeclareLaunchArgument( + "robot_xacro_path", + default_value=[ + FindPackageShare('robotnik_description'), '/robots/', + LaunchConfiguration('robot'), '/', LaunchConfiguration('robot_model'), '.urdf.xacro', + ], + description="Path to Robot Xacro File" + ), + DeclareLaunchArgument( + "low_performance_simulation", + default_value="true", + description="Enable smooth simulation for low performance computers" + ), + DeclareLaunchArgument( + "use_rviz", + default_value="true", + description="Enable rviz gui" + ), + DeclareLaunchArgument( + "run_moveit", + default_value="false", + description="Enable MoveIt for manipulation" + ), + DeclareLaunchArgument( + "run_localization", + default_value="true", + description="Enable localization stack" + ), + DeclareLaunchArgument( + "run_navigation", + default_value="true", + description="Enable navigation stack" + ), + DeclareLaunchArgument( + "arm_type", + default_value="ur10e", + description="Type of robotic arm" + ), + DeclareLaunchArgument( + "x", + default_value="0.0", + description="Robot initial position in x" + ), + DeclareLaunchArgument( + "y", + default_value="0.0", + description="Robot initial position in y" + ), + DeclareLaunchArgument( + "z", + default_value="0.0", + description="Robot initial position in z" + ), + ] + + robot_id = LaunchConfiguration("robot_id") + robot = LaunchConfiguration("robot") + robot_model = LaunchConfiguration("robot_model") + robot_xacro_path = LaunchConfiguration("robot_xacro_path") + low_performance_simulation = LaunchConfiguration("low_performance_simulation") + use_rviz = LaunchConfiguration("use_rviz") + run_moveit = LaunchConfiguration("run_moveit") + run_localization = LaunchConfiguration("run_localization") + run_navigation = LaunchConfiguration("run_navigation") + arm_type = LaunchConfiguration("arm_type") + + gazebo_robot = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('robotnik_gazebo_ignition'), 'launch/spawn_robot.launch.py' + ]) + ), + launch_arguments={ + 'robot_id': robot_id, + 'robot': robot, + 'robot_model': robot_model, + 'robot_xacro_path': robot_xacro_path, + 'arm_type': arm_type, + 'low_performance_simulation': low_performance_simulation, + 'run_rviz': 'false' + }.items() + ) + + # In spawn_robot.launch.py the add_laser("front") is defined for any robot model + # This causes two publishers to /robot/front_laser/scan when laser filters are enabled + # In rbsummit and rbwatcher is not an issue because front laser is not available + # but in other robot models that have front laser, it causes conflict. + laser_filters = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('robotnik_simulation_bringup'), 'launch/laser_filters.launch.py' + ]) + ), + launch_arguments={ + 'robot_id': robot_id, + 'use_sim': 'true', + }.items(), + condition=IfCondition( + OrSubstitution( + OrSubstitution( + EqualsSubstitution(robot_model, 'rbsummit'), + EqualsSubstitution(robot_model, 'rbwatcher'), + ), + EqualsSubstitution(robot_model, 'rbcar'), + ) + ) + ) + + localization = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('robotnik_simulation_localization'), 'launch/localization.launch.py' + ]) + ), + launch_arguments={ + 'robot_id': robot_id, + 'use_sim': 'true', + }.items(), + condition=IfCondition(run_localization), + ) + + delayed_localization = TimerAction( + period=10.0, + actions=[localization] + ) + + navigation = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('robotnik_simulation_navigation'), 'launch/navigation.launch.py' + ]) + ), + launch_arguments={ + 'robot_id': robot_id, + 'use_sim': 'true', + }.items(), + condition=IfCondition(run_navigation), + ) + + delayed_navigation = TimerAction( + period=15.0, + actions=[navigation] + ) + + rviz = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('robotnik_simulation_bringup'), 'launch/rviz.launch.py' + ]) + ), + condition=IfCondition(use_rviz) + ) + + delayed_rviz = TimerAction( + period=20.0, + actions=[rviz] + ) + + moveit = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('robotnik_simulation_moveit'), 'launch/moveit.launch.py', + ]) + ), + launch_arguments={ + 'robot_id': robot_id, + 'robot': robot, + 'robot_model': robot_model, + 'robot_xacro_path': robot_xacro_path, + 'moveit_config_name': [robot, '_moveit_config'], + 'arm_type': arm_type, + 'use_sim_time': 'true', + }.items(), + condition=IfCondition(run_moveit), + ) + + delayed_moveit = TimerAction( + period=25.0, + actions=[moveit] + ) + + group = GroupAction([ + gazebo_robot, + laser_filters, + delayed_localization, + delayed_navigation, + delayed_rviz, + delayed_moveit, + ]) + + return LaunchDescription(declared_arguments + [group]) \ No newline at end of file diff --git a/common/robotnik_simulation_localization/config/amcl.yaml b/common/robotnik_simulation_localization/config/amcl.yaml index df9ca86..c28ef17 100644 --- a/common/robotnik_simulation_localization/config/amcl.yaml +++ b/common/robotnik_simulation_localization/config/amcl.yaml @@ -3,11 +3,11 @@ amcl: ros__parameters: use_sim_time: true - base_frame_id: robot_base_footprint - global_frame_id: robot_map - odom_frame_id: robot_odom + base_frame_id: $(var robot_id)_base_footprint + global_frame_id: $(var robot_id)_map + odom_frame_id: $(var robot_id)_odom robot_model_type: nav2_amcl::DifferentialMotionModel - scan_topic: /robot/front_laser/scan + scan_topic: /$(var robot_id)/front_laser/scan set_initial_pose: true initial_pose: diff --git a/common/robotnik_simulation_localization/config/slam_toolbox.yaml b/common/robotnik_simulation_localization/config/slam_toolbox.yaml index 492c20c..28c2399 100644 --- a/common/robotnik_simulation_localization/config/slam_toolbox.yaml +++ b/common/robotnik_simulation_localization/config/slam_toolbox.yaml @@ -12,11 +12,11 @@ ceres_loss_function: None # ROS Parameters - odom_frame: robot_odom - map_frame: robot_map - base_frame: robot_base_footprint + odom_frame: $(var robot_id)_odom + map_frame: $(var robot_id)_map + base_frame: $(var robot_id)_base_footprint map_name: map - scan_topic: /robot/front_laser/scan + scan_topic: /$(var robot_id)/front_laser/scan # localization mode: mapping diff --git a/common/robotnik_simulation_localization/launch/localization_2d.launch.py b/common/robotnik_simulation_localization/launch/localization_2d.launch.py index 0e5200a..649481a 100644 --- a/common/robotnik_simulation_localization/launch/localization_2d.launch.py +++ b/common/robotnik_simulation_localization/launch/localization_2d.launch.py @@ -27,7 +27,7 @@ from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.substitutions import FindPackageShare from launch_ros.actions import Node -from launch.substitutions import LaunchConfiguration +from launch_ros.parameter_descriptions import ParameterFile from launch.actions import GroupAction def generate_launch_description(): @@ -40,10 +40,13 @@ def generate_launch_description(): 'maps/demo_map/demo_map.yaml' ]) - amcl_config = PathJoinSubstitution([ - FindPackageShare('robotnik_simulation_localization'), - 'config/amcl.yaml' - ]) + amcl_config = ParameterFile( + PathJoinSubstitution([ + FindPackageShare('robotnik_simulation_localization'), + 'config/amcl.yaml' + ]), + allow_substs=True + ) map_server = Node( package='nav2_map_server', @@ -54,7 +57,7 @@ def generate_launch_description(): { 'use_sim_time': use_sim, 'yaml_filename': map_file, - 'frame_id': 'robot_map' + 'frame_id': [robot_id, '_map'] } ] ) diff --git a/common/robotnik_simulation_localization/launch/mapping_2d.launch.py b/common/robotnik_simulation_localization/launch/mapping_2d.launch.py index 53e22dc..8c016e4 100644 --- a/common/robotnik_simulation_localization/launch/mapping_2d.launch.py +++ b/common/robotnik_simulation_localization/launch/mapping_2d.launch.py @@ -27,6 +27,7 @@ from launch import LaunchDescription from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.substitutions import FindPackageShare +from launch_ros.parameter_descriptions import ParameterFile from launch.actions import GroupAction def generate_launch_description(): @@ -34,10 +35,13 @@ def generate_launch_description(): robot_id = LaunchConfiguration("robot_id") use_sim = LaunchConfiguration("use_sim", default="true") - slam_toolbox_config = PathJoinSubstitution([ - FindPackageShare('robotnik_simulation_localization'), - 'config/slam_toolbox.yaml' - ]) + slam_toolbox_config = ParameterFile( + PathJoinSubstitution([ + FindPackageShare('robotnik_simulation_localization'), + 'config/slam_toolbox.yaml' + ]), + allow_substs=True + ) slam_toolbox_mapping = Node( package='slam_toolbox', diff --git a/common/robotnik_simulation_navigation/config/behavior_server.yaml b/common/robotnik_simulation_navigation/config/behavior_server.yaml index 1267e72..cdd99b2 100644 --- a/common/robotnik_simulation_navigation/config/behavior_server.yaml +++ b/common/robotnik_simulation_navigation/config/behavior_server.yaml @@ -16,9 +16,9 @@ plugin: "nav2_behaviors::DriveOnHeading" wait: plugin: "nav2_behaviors::Wait" - local_frame: robot_odom - global_frame: robot_map - robot_base_frame: robot_base_link + local_frame: $(var robot_id)_odom + global_frame: $(var robot_id)_map + robot_base_frame: $(var robot_id)_base_link transform_tolerance: 0.1 use_sim_time: true simulate_ahead_time: 2.0 diff --git a/common/robotnik_simulation_navigation/config/bt_navigator.yaml b/common/robotnik_simulation_navigation/config/bt_navigator.yaml index 6af6a57..f810285 100644 --- a/common/robotnik_simulation_navigation/config/bt_navigator.yaml +++ b/common/robotnik_simulation_navigation/config/bt_navigator.yaml @@ -1,8 +1,8 @@ /**: bt_navigator: ros__parameters: - global_frame: robot_map - robot_base_frame: robot_base_link + global_frame: $(var robot_id)_map + robot_base_frame: $(var robot_id)_base_link bt_loop_duration: 10 default_server_timeout: 20 wait_for_service_timeout: 1000 diff --git a/common/robotnik_simulation_navigation/config/controller_server.yaml b/common/robotnik_simulation_navigation/config/controller_server.yaml index 3086721..fbf6c6a 100644 --- a/common/robotnik_simulation_navigation/config/controller_server.yaml +++ b/common/robotnik_simulation_navigation/config/controller_server.yaml @@ -7,7 +7,7 @@ min_y_velocity_threshold: 0.001 min_theta_velocity_threshold: 0.001 failure_tolerance: 0.3 - odom_topic: /robot/robotnik_base_control/odom + odom_topic: /$(var robot_id)/robotnik_base_control/odom progress_checker_plugin: "progress_checker" goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" controller_plugins: ["FollowPath"] @@ -82,8 +82,8 @@ use_sim_time: true update_frequency: 10.0 publish_frequency: 5.0 - global_frame: robot_odom - robot_base_frame: robot_base_link + global_frame: $(var robot_id)_odom + robot_base_frame: $(var robot_id)_base_link rolling_window: true width: 8 height: 8 @@ -98,9 +98,23 @@ footprint_clearing_enabled: false max_obstacle_height: 2.0 combination_method: 1 - observation_sources: front_laser + observation_sources: front_laser rear_laser front_laser: - topic: /robot/front_laser/scan + topic: /$(var robot_id)/front_laser/scan + observation_persistence: 0.5 + expected_update_rate: 10.0 + data_type: "LaserScan" + min_obstacle_height: 0.0 + max_obstacle_height: 1.0 + inf_is_valid: true + marking: true + clearing: true + obstacle_max_range: 4.0 + obstacle_min_range: 0.0 + raytrace_max_range: 5.5 + raytrace_min_range: 0.0 + rear_laser: + topic: /$(var robot_id)/rear_laser/scan observation_persistence: 0.5 expected_update_rate: 10.0 data_type: "LaserScan" diff --git a/common/robotnik_simulation_navigation/config/planner_server.yaml b/common/robotnik_simulation_navigation/config/planner_server.yaml index 9fad387..90bb910 100644 --- a/common/robotnik_simulation_navigation/config/planner_server.yaml +++ b/common/robotnik_simulation_navigation/config/planner_server.yaml @@ -15,8 +15,8 @@ ros__parameters: update_frequency: 1.0 publish_frequency: 1.0 - global_frame: robot_map - robot_base_frame: robot_base_link + global_frame: $(var robot_id)_map + robot_base_frame: $(var robot_id)_base_link footprint: "[[0.4,-0.35],[0.4,0.35],[-0.4,0.35],[-0.4,-0.35]]" footprint_padding: 0.05 # robot_radius: 0.6 #0.60 @@ -24,7 +24,7 @@ track_unknown_space: true plugins: ["static_layer", "inflation_layer"] static_layer: - map_topic: /robot/map + map_topic: /$(var robot_id)/map plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: true inflation_layer: diff --git a/common/robotnik_simulation_navigation/config/route_server.yaml b/common/robotnik_simulation_navigation/config/route_server.yaml index 77f26f4..208478e 100644 --- a/common/robotnik_simulation_navigation/config/route_server.yaml +++ b/common/robotnik_simulation_navigation/config/route_server.yaml @@ -9,8 +9,8 @@ radius_to_achieve_node: 0.1 smooth_corners: true smoothing_radius: 0.2 - route_frame: robot_map - base_frame: robot_base_link + route_frame: $(var robot_id)_map + base_frame: $(var robot_id)_base_link operations: ["AdjustSpeedLimit", "ReroutingService", "CollisionMonitor"] ReroutingService: plugin: "nav2_route::ReroutingService" diff --git a/common/robotnik_simulation_navigation/config/smoother_server.yaml b/common/robotnik_simulation_navigation/config/smoother_server.yaml index e875506..372ff14 100644 --- a/common/robotnik_simulation_navigation/config/smoother_server.yaml +++ b/common/robotnik_simulation_navigation/config/smoother_server.yaml @@ -1,7 +1,7 @@ /**: smoother_server: ros__parameters: - robot_base_frame: robot_base_link + robot_base_frame: $(var robot_id)_base_link smoother_plugins: ["simple_smoother", "route_smoother"] simple_smoother: plugin: "nav2_smoother::SimpleSmoother" diff --git a/common/robotnik_simulation_navigation/launch/nav2_mission.launch.py b/common/robotnik_simulation_navigation/launch/nav2_mission.launch.py index 737d1f6..f0773f6 100644 --- a/common/robotnik_simulation_navigation/launch/nav2_mission.launch.py +++ b/common/robotnik_simulation_navigation/launch/nav2_mission.launch.py @@ -28,6 +28,7 @@ from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.substitutions import FindPackageShare from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterFile from launch.actions import GroupAction def generate_launch_description(): @@ -35,15 +36,21 @@ def generate_launch_description(): robot_id = LaunchConfiguration("robot_id") use_sim = LaunchConfiguration("use_sim") - waypoint_config = PathJoinSubstitution([ - FindPackageShare('robotnik_simulation_navigation'), - 'config/waypoint_follower.yaml' - ]) + waypoint_config = ParameterFile( + PathJoinSubstitution([ + FindPackageShare('robotnik_simulation_navigation'), + 'config/waypoint_follower.yaml' + ]), + allow_substs=True + ) - route_config = PathJoinSubstitution([ - FindPackageShare('robotnik_simulation_navigation'), - 'config/route_server.yaml' - ]) + route_config = ParameterFile( + PathJoinSubstitution([ + FindPackageShare('robotnik_simulation_navigation'), + 'config/route_server.yaml' + ]), + allow_substs=True + ) route_graph_filepath = PathJoinSubstitution([ FindPackageShare('robotnik_simulation_navigation'), diff --git a/common/robotnik_simulation_navigation/launch/nav2_task.launch.py b/common/robotnik_simulation_navigation/launch/nav2_task.launch.py index 75f75fd..3d8c3e8 100644 --- a/common/robotnik_simulation_navigation/launch/nav2_task.launch.py +++ b/common/robotnik_simulation_navigation/launch/nav2_task.launch.py @@ -27,6 +27,7 @@ from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.substitutions import FindPackageShare from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterFile from launch.actions import GroupAction def generate_launch_description(): @@ -36,34 +37,49 @@ def generate_launch_description(): # Nav2 core node configurations - controller_config = PathJoinSubstitution([ - FindPackageShare('robotnik_simulation_navigation'), - 'config/controller_server.yaml' - ]) + controller_config = ParameterFile( + PathJoinSubstitution([ + FindPackageShare('robotnik_simulation_navigation'), + 'config/controller_server.yaml' + ]), + allow_substs=True + ) - planner_config = PathJoinSubstitution([ - FindPackageShare('robotnik_simulation_navigation'), - 'config/planner_server.yaml' - ]) + planner_config = ParameterFile( + PathJoinSubstitution([ + FindPackageShare('robotnik_simulation_navigation'), + 'config/planner_server.yaml' + ]), + allow_substs=True + ) # Nav2 auxiliary node configurations - behavior_config = PathJoinSubstitution([ - FindPackageShare('robotnik_simulation_navigation'), - 'config/behavior_server.yaml' - ]) + behavior_config = ParameterFile( + PathJoinSubstitution([ + FindPackageShare('robotnik_simulation_navigation'), + 'config/behavior_server.yaml' + ]), + allow_substs=True + ) - smoother_config = PathJoinSubstitution([ - FindPackageShare('robotnik_simulation_navigation'), - 'config/smoother_server.yaml' - ]) + smoother_config = ParameterFile( + PathJoinSubstitution([ + FindPackageShare('robotnik_simulation_navigation'), + 'config/smoother_server.yaml' + ]), + allow_substs=True + ) # Nav2 orchestration node configurations - bt_navigator_config = PathJoinSubstitution([ - FindPackageShare('robotnik_simulation_navigation'), - 'config/bt_navigator.yaml' - ]) + bt_navigator_config = ParameterFile( + PathJoinSubstitution([ + FindPackageShare('robotnik_simulation_navigation'), + 'config/bt_navigator.yaml' + ]), + allow_substs=True + ) bt_navigator_pose_xml = PathJoinSubstitution([ FindPackageShare('robotnik_simulation_navigation'),