From 720cb0ceef4dea2eb0787bc1d848d2a91d8f2b66 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz <61198661+ygoumaz@users.noreply.github.com> Date: Wed, 7 Jun 2023 14:58:25 +0200 Subject: [PATCH] Cleanup simulation reset in launch files (#764) * launch files * changelogs * restore supervisor for clock topic * update universal robot * Update CHANGELOG.rst --- webots_ros2/CHANGELOG.rst | 1 + webots_ros2_epuck/CHANGELOG.rst | 1 + webots_ros2_epuck/launch/robot_launch.py | 88 +++++++---------- webots_ros2_mavic/CHANGELOG.rst | 4 + webots_ros2_mavic/launch/robot_launch.py | 47 +++------ webots_ros2_tesla/CHANGELOG.rst | 4 + webots_ros2_tesla/launch/robot_launch.py | 50 +++------- webots_ros2_tiago/CHANGELOG.rst | 1 + webots_ros2_tiago/launch/robot_launch.py | 98 ++++++++----------- webots_ros2_turtlebot/CHANGELOG.rst | 4 + webots_ros2_turtlebot/launch/robot_launch.py | 98 +++++++------------ webots_ros2_universal_robot/CHANGELOG.rst | 4 + .../launch/multirobot_launch.py | 82 ++++++++-------- 13 files changed, 199 insertions(+), 283 deletions(-) diff --git a/webots_ros2/CHANGELOG.rst b/webots_ros2/CHANGELOG.rst index 41fe97b86..b496aedec 100644 --- a/webots_ros2/CHANGELOG.rst +++ b/webots_ros2/CHANGELOG.rst @@ -5,6 +5,7 @@ Changelog for package webots_ros2 2023.1.0 (2023-XX-XX) ------------------ * Added support for Navigation2 in Iron. +* Clean simulation reset in launch files. 2023.0.4 (2023-05-23) ------------------ diff --git a/webots_ros2_epuck/CHANGELOG.rst b/webots_ros2_epuck/CHANGELOG.rst index e58591f22..8a08ae8eb 100644 --- a/webots_ros2_epuck/CHANGELOG.rst +++ b/webots_ros2_epuck/CHANGELOG.rst @@ -5,6 +5,7 @@ Changelog for package webots_ros2_epuck 2023.1.0 (2023-XX-XX) ------------------ * Added support for Navigation2 in Iron. +* Clean simulation reset in launch file. 2023.0.4 (2023-05-23) ------------------ diff --git a/webots_ros2_epuck/launch/robot_launch.py b/webots_ros2_epuck/launch/robot_launch.py index ec56e87f1..208dda13e 100644 --- a/webots_ros2_epuck/launch/robot_launch.py +++ b/webots_ros2_epuck/launch/robot_launch.py @@ -31,17 +31,37 @@ from webots_ros2_driver.utils import controller_url_prefix -def get_ros2_nodes(*args): +def generate_launch_description(): package_dir = get_package_share_directory('webots_ros2_epuck') + world = LaunchConfiguration('world') use_nav = LaunchConfiguration('nav', default=False) use_rviz = LaunchConfiguration('rviz', default=False) use_mapper = LaunchConfiguration('mapper', default=False) fill_map = LaunchConfiguration('fill_map', default=True) map_filename = LaunchConfiguration('map', default=os.path.join(package_dir, 'resource', 'epuck_world_map.yaml')) - robot_description = pathlib.Path(os.path.join(package_dir, 'resource', 'epuck_webots.urdf')).read_text() - ros2_control_params = os.path.join(package_dir, 'resource', 'ros2_control.yml') use_sim_time = LaunchConfiguration('use_sim_time', default=True) + webots = WebotsLauncher( + world=PathJoinSubstitution([package_dir, 'worlds', world]), + ros2_supervisor=True + ) + + robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[{ + 'robot_description': '' + }], + ) + + footprint_publisher = Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_footprint'], + ) + # ROS control spawners controller_manager_timeout = ['--controller-manager-timeout', '50'] controller_manager_prefix = 'python.exe' if os.name == 'nt' else '' @@ -67,6 +87,8 @@ def get_ros2_nodes(*args): ) ros_control_spawners = [diffdrive_controller_spawner, joint_state_broadcaster_spawner] + robot_description = pathlib.Path(os.path.join(package_dir, 'resource', 'epuck_webots.urdf')).read_text() + ros2_control_params = os.path.join(package_dir, 'resource', 'ros2_control.yml') mappings = [('/diffdrive_controller/cmd_vel_unstamped', '/cmd_vel'), ('/diffdrive_controller/odom', '/odom')] epuck_driver = Node( package='webots_ros2_driver', @@ -91,22 +113,6 @@ def get_ros2_nodes(*args): ], ) - robot_state_publisher = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - output='screen', - parameters=[{ - 'robot_description': '' - }], - ) - - footprint_publisher = Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_footprint'], - ) - # Tools tool_nodes = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -128,33 +134,6 @@ def get_ros2_nodes(*args): nodes_to_start=[tool_nodes] + ros_control_spawners ) - return [ - robot_state_publisher, - epuck_driver, - footprint_publisher, - epuck_process, - waiting_nodes, - ] - - -def generate_launch_description(): - package_dir = get_package_share_directory('webots_ros2_epuck') - world = LaunchConfiguration('world') - - webots = WebotsLauncher( - world=PathJoinSubstitution([package_dir, 'worlds', world]), - ros2_supervisor=True - ) - - # The following line is important! - # This event handler respawns the ROS 2 nodes on simulation reset (supervisor process ends). - reset_handler = launch.actions.RegisterEventHandler( - event_handler=launch.event_handlers.OnProcessExit( - target_action=webots._supervisor, - on_exit=get_ros2_nodes, - ) - ) - return LaunchDescription([ DeclareLaunchArgument( 'world', @@ -164,19 +143,20 @@ def generate_launch_description(): webots, webots._supervisor, + robot_state_publisher, + footprint_publisher, + + epuck_driver, + epuck_process, + waiting_nodes, + # This action will kill all nodes once the Webots simulation has exited launch.actions.RegisterEventHandler( event_handler=launch.event_handlers.OnProcessExit( target_action=webots, on_exit=[ - launch.actions.UnregisterEventHandler( - event_handler=reset_handler.event_handler - ), launch.actions.EmitEvent(event=launch.events.Shutdown()) ], ) - ), - - # Add the reset event handler - reset_handler - ] + get_ros2_nodes()) + ) + ]) diff --git a/webots_ros2_mavic/CHANGELOG.rst b/webots_ros2_mavic/CHANGELOG.rst index fdfc6e1d0..8856d2481 100644 --- a/webots_ros2_mavic/CHANGELOG.rst +++ b/webots_ros2_mavic/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package webots_ros2_mavic ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2023.1.0 (2023-XX-XX) +------------------ +* Clean simulation reset in launch file. + 2023.0.2 (2023-02-07) ------------------ * Updated supervisor launch. diff --git a/webots_ros2_mavic/launch/robot_launch.py b/webots_ros2_mavic/launch/robot_launch.py index 8f346d7ff..266dd4e77 100644 --- a/webots_ros2_mavic/launch/robot_launch.py +++ b/webots_ros2_mavic/launch/robot_launch.py @@ -29,25 +29,6 @@ from webots_ros2_driver.utils import controller_url_prefix -def get_ros2_nodes(*args): - package_dir = get_package_share_directory('webots_ros2_mavic') - robot_description = pathlib.Path(os.path.join(package_dir, 'resource', 'mavic_webots.urdf')).read_text() - - mavic_driver = Node( - package='webots_ros2_driver', - executable='driver', - output='screen', - additional_env={'WEBOTS_CONTROLLER_URL': controller_url_prefix() + 'Mavic_2_PRO'}, - parameters=[ - {'robot_description': robot_description}, - ] - ) - - return [ - mavic_driver, - ] - - def generate_launch_description(): package_dir = get_package_share_directory('webots_ros2_mavic') world = LaunchConfiguration('world') @@ -57,13 +38,16 @@ def generate_launch_description(): ros2_supervisor=True ) - # The following line is important! - # This event handler respawns the ROS 2 nodes on simulation reset (supervisor process ends). - reset_handler = launch.actions.RegisterEventHandler( - event_handler=launch.event_handlers.OnProcessExit( - target_action=webots._supervisor, - on_exit=get_ros2_nodes, - ) + robot_description = pathlib.Path(os.path.join(package_dir, 'resource', 'mavic_webots.urdf')).read_text() + mavic_driver = Node( + package='webots_ros2_driver', + executable='driver', + output='screen', + additional_env={'WEBOTS_CONTROLLER_URL': controller_url_prefix() + 'Mavic_2_PRO'}, + parameters=[ + {'robot_description': robot_description}, + ], + respawn=True ) return LaunchDescription([ @@ -74,20 +58,15 @@ def generate_launch_description(): ), webots, webots._supervisor, + mavic_driver, # This action will kill all nodes once the Webots simulation has exited launch.actions.RegisterEventHandler( event_handler=launch.event_handlers.OnProcessExit( target_action=webots, on_exit=[ - launch.actions.UnregisterEventHandler( - event_handler=reset_handler.event_handler - ), launch.actions.EmitEvent(event=launch.events.Shutdown()) ], ) - ), - - # Add the reset event handler - reset_handler - ] + get_ros2_nodes()) + ) + ]) diff --git a/webots_ros2_tesla/CHANGELOG.rst b/webots_ros2_tesla/CHANGELOG.rst index 3ab41133e..dea156ed3 100644 --- a/webots_ros2_tesla/CHANGELOG.rst +++ b/webots_ros2_tesla/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package webots_ros2_tesla ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2023.1.0 (2023-XX-XX) +------------------ +* Clean simulation reset in launch file. + 2023.0.2 (2023-02-07) ------------------ * Updated supervisor launch. diff --git a/webots_ros2_tesla/launch/robot_launch.py b/webots_ros2_tesla/launch/robot_launch.py index bc1d24c15..6409add59 100644 --- a/webots_ros2_tesla/launch/robot_launch.py +++ b/webots_ros2_tesla/launch/robot_launch.py @@ -29,10 +29,16 @@ from webots_ros2_driver.utils import controller_url_prefix -def get_ros2_nodes(*args): +def generate_launch_description(): package_dir = get_package_share_directory('webots_ros2_tesla') - robot_description = pathlib.Path(os.path.join(package_dir, 'resource', 'tesla_webots.urdf')).read_text() + world = LaunchConfiguration('world') + + webots = WebotsLauncher( + world=PathJoinSubstitution([package_dir, 'worlds', world]), + ros2_supervisor=True + ) + robot_description = pathlib.Path(os.path.join(package_dir, 'resource', 'tesla_webots.urdf')).read_text() tesla_driver = Node( package='webots_ros2_driver', executable='driver', @@ -40,38 +46,14 @@ def get_ros2_nodes(*args): additional_env={'WEBOTS_CONTROLLER_URL': controller_url_prefix() + 'vehicle'}, parameters=[ {'robot_description': robot_description}, - ] + ], + respawn=True ) - lane_follower = Node( package='webots_ros2_tesla', executable='lane_follower', ) - return [ - lane_follower, - tesla_driver, - ] - - -def generate_launch_description(): - package_dir = get_package_share_directory('webots_ros2_tesla') - world = LaunchConfiguration('world') - - webots = WebotsLauncher( - world=PathJoinSubstitution([package_dir, 'worlds', world]), - ros2_supervisor=True - ) - - # The following line is important! - # This event handler respawns the ROS 2 nodes on simulation reset (supervisor process ends). - reset_handler = launch.actions.RegisterEventHandler( - event_handler=launch.event_handlers.OnProcessExit( - target_action=webots._supervisor, - on_exit=get_ros2_nodes, - ) - ) - return LaunchDescription([ DeclareLaunchArgument( 'world', @@ -80,20 +62,16 @@ def generate_launch_description(): ), webots, webots._supervisor, + tesla_driver, + lane_follower, # This action will kill all nodes once the Webots simulation has exited launch.actions.RegisterEventHandler( event_handler=launch.event_handlers.OnProcessExit( target_action=webots, on_exit=[ - launch.actions.UnregisterEventHandler( - event_handler=reset_handler.event_handler - ), launch.actions.EmitEvent(event=launch.events.Shutdown()) ], ) - ), - - # Add the reset event handler - reset_handler - ] + get_ros2_nodes()) + ) + ]) diff --git a/webots_ros2_tiago/CHANGELOG.rst b/webots_ros2_tiago/CHANGELOG.rst index 4e9295a77..d75b443f9 100644 --- a/webots_ros2_tiago/CHANGELOG.rst +++ b/webots_ros2_tiago/CHANGELOG.rst @@ -5,6 +5,7 @@ Changelog for package webots_ros2_tiago 2023.1.0 (2023-XX-XX) ------------------ * Added support for Navigation2 in Iron. +* Clean simulation reset in launch file. 2023.0.4 (2023-05-23) ------------------ diff --git a/webots_ros2_tiago/launch/robot_launch.py b/webots_ros2_tiago/launch/robot_launch.py index 2e3104237..019ab01f2 100644 --- a/webots_ros2_tiago/launch/robot_launch.py +++ b/webots_ros2_tiago/launch/robot_launch.py @@ -32,20 +32,38 @@ from webots_ros2_driver.utils import controller_url_prefix -def get_ros2_nodes(*args): +def generate_launch_description(): package_dir = get_package_share_directory('webots_ros2_tiago') + world = LaunchConfiguration('world') + mode = LaunchConfiguration('mode') use_rviz = LaunchConfiguration('rviz', default=False) use_nav = LaunchConfiguration('nav', default=False) use_slam_toolbox = LaunchConfiguration('slam_toolbox', default=False) use_slam_cartographer = LaunchConfiguration('slam_cartographer', default=False) - robot_description = pathlib.Path(os.path.join(package_dir, 'resource', 'tiago_webots.urdf')).read_text() - ros2_control_params = os.path.join(package_dir, 'resource', 'ros2_control.yml') - toolbox_params = os.path.join(package_dir, 'resource', 'slam_toolbox_params.yaml') - nav2_map = os.path.join(package_dir, 'resource', 'map.yaml') - cartographer_config_dir = os.path.join(package_dir, 'resource') - cartographer_config_basename = 'cartographer.lua' use_sim_time = LaunchConfiguration('use_sim_time', default=True) + webots = WebotsLauncher( + world=PathJoinSubstitution([package_dir, 'worlds', world]), + mode=mode, + ros2_supervisor=True + ) + + robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[{ + 'robot_description': '' + }], + ) + + footprint_publisher = Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_footprint'], + ) + # ROS control spawners controller_manager_timeout = ['--controller-manager-timeout', '500'] controller_manager_prefix = 'python.exe' if os.name == 'nt' else '' @@ -65,6 +83,8 @@ def get_ros2_nodes(*args): ) ros_control_spawners = [diffdrive_controller_spawner, joint_state_broadcaster_spawner] + robot_description = pathlib.Path(os.path.join(package_dir, 'resource', 'tiago_webots.urdf')).read_text() + ros2_control_params = os.path.join(package_dir, 'resource', 'ros2_control.yml') mappings = [('/diffdrive_controller/cmd_vel_unstamped', '/cmd_vel'), ('/diffdrive_controller/odom', '/odom')] tiago_driver = Node( package='webots_ros2_driver', @@ -80,22 +100,6 @@ def get_ros2_nodes(*args): remappings=mappings ) - robot_state_publisher = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - output='screen', - parameters=[{ - 'robot_description': '' - }], - ) - - footprint_publisher = Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_footprint'], - ) - rviz_config = os.path.join(get_package_share_directory('webots_ros2_tiago'), 'resource', 'default.rviz') rviz = Node( package='rviz2', @@ -111,6 +115,7 @@ def get_ros2_nodes(*args): nav2_params_file = 'nav2_params_iron.yaml' if ('ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] == 'iron') else 'nav2_params.yaml' nav2_params = os.path.join(package_dir, 'resource', nav2_params_file) + nav2_map = os.path.join(package_dir, 'resource', 'map.yaml') if 'nav2_bringup' in get_packages_with_prefixes(): navigation_nodes.append(IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join( @@ -123,6 +128,8 @@ def get_ros2_nodes(*args): condition=launch.conditions.IfCondition(use_nav))) # SLAM + cartographer_config_dir = os.path.join(package_dir, 'resource') + cartographer_config_basename = 'cartographer.lua' cartographer = Node( package='cartographer_ros', executable='cartographer_node', @@ -145,6 +152,7 @@ def get_ros2_nodes(*args): condition=launch.conditions.IfCondition(use_slam_cartographer)) navigation_nodes.append(cartographer_grid) + toolbox_params = os.path.join(package_dir, 'resource', 'slam_toolbox_params.yaml') slam_toolbox = Node( parameters=[toolbox_params, {'use_sim_time': use_sim_time}], @@ -162,34 +170,6 @@ def get_ros2_nodes(*args): nodes_to_start=[rviz] + navigation_nodes + ros_control_spawners ) - return [ - robot_state_publisher, - tiago_driver, - footprint_publisher, - waiting_nodes, - ] - - -def generate_launch_description(): - package_dir = get_package_share_directory('webots_ros2_tiago') - world = LaunchConfiguration('world') - mode = LaunchConfiguration('mode') - - webots = WebotsLauncher( - world=PathJoinSubstitution([package_dir, 'worlds', world]), - mode=mode, - ros2_supervisor=True - ) - - # The following line is important! - # This event handler respawns the ROS 2 nodes on simulation reset (supervisor process ends). - reset_handler = launch.actions.RegisterEventHandler( - event_handler=launch.event_handlers.OnProcessExit( - target_action=webots._supervisor, - on_exit=get_ros2_nodes, - ) - ) - return LaunchDescription([ DeclareLaunchArgument( 'world', @@ -204,19 +184,19 @@ def generate_launch_description(): webots, webots._supervisor, + robot_state_publisher, + footprint_publisher, + + tiago_driver, + waiting_nodes, + # This action will kill all nodes once the Webots simulation has exited launch.actions.RegisterEventHandler( event_handler=launch.event_handlers.OnProcessExit( target_action=webots, on_exit=[ - launch.actions.UnregisterEventHandler( - event_handler=reset_handler.event_handler - ), launch.actions.EmitEvent(event=launch.events.Shutdown()) ], ) - ), - - # Add the reset event handler - reset_handler - ] + get_ros2_nodes()) + ) + ]) diff --git a/webots_ros2_turtlebot/CHANGELOG.rst b/webots_ros2_turtlebot/CHANGELOG.rst index 946fbb10d..e4684da43 100644 --- a/webots_ros2_turtlebot/CHANGELOG.rst +++ b/webots_ros2_turtlebot/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package webots_ros2_turtlebot ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2023.1.0 (2023-XX-XX) +------------------ +* Clean simulation reset in launch file. + 2023.0.4 (2023-05-23) ------------------ * Start ros control and navigation nodes when Webots is ready. diff --git a/webots_ros2_turtlebot/launch/robot_launch.py b/webots_ros2_turtlebot/launch/robot_launch.py index 7f9900bce..632f811bf 100644 --- a/webots_ros2_turtlebot/launch/robot_launch.py +++ b/webots_ros2_turtlebot/launch/robot_launch.py @@ -32,17 +32,36 @@ from webots_ros2_driver.utils import controller_url_prefix -def get_ros2_nodes(*args): +def generate_launch_description(): package_dir = get_package_share_directory('webots_ros2_turtlebot') + world = LaunchConfiguration('world') + mode = LaunchConfiguration('mode') use_nav = LaunchConfiguration('nav', default=False) use_slam = LaunchConfiguration('slam', default=False) - robot_description = pathlib.Path(os.path.join(package_dir, 'resource', 'turtlebot_webots.urdf')).read_text() - ros2_control_params = os.path.join(package_dir, 'resource', 'ros2control.yml') - nav2_params = os.path.join(package_dir, 'resource', 'nav2_params.yaml') - nav2_map = os.path.join(package_dir, 'resource', 'turtlebot3_burger_example_map.yaml') use_sim_time = LaunchConfiguration('use_sim_time', default=True) - # TODO: Revert once the https://github.com/ros-controls/ros2_control/pull/444 PR gets into the release + webots = WebotsLauncher( + world=PathJoinSubstitution([package_dir, 'worlds', world]), + mode=mode, + ros2_supervisor=True + ) + + robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[{ + 'robot_description': '' + }], + ) + + footprint_publisher = Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_footprint'], + ) + # ROS control spawners controller_manager_timeout = ['--controller-manager-timeout', '50'] controller_manager_prefix = 'python.exe' if os.name == 'nt' else '' @@ -62,10 +81,9 @@ def get_ros2_nodes(*args): ) ros_control_spawners = [diffdrive_controller_spawner, joint_state_broadcaster_spawner] - mappings = [('/diffdrive_controller/cmd_vel_unstamped', '/cmd_vel')] - if 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] in ['humble', 'rolling']: - mappings.append(('/diffdrive_controller/odom', '/odom')) - + robot_description = pathlib.Path(os.path.join(package_dir, 'resource', 'turtlebot_webots.urdf')).read_text() + ros2_control_params = os.path.join(package_dir, 'resource', 'ros2control.yml') + mappings = [('/diffdrive_controller/cmd_vel_unstamped', '/cmd_vel'), ('/diffdrive_controller/odom', '/odom')] turtlebot_driver = Node( package='webots_ros2_driver', executable='driver', @@ -80,25 +98,11 @@ def get_ros2_nodes(*args): remappings=mappings ) - robot_state_publisher = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - output='screen', - parameters=[{ - 'robot_description': '' - }], - ) - - footprint_publisher = Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_footprint'], - ) - # Navigation navigation_nodes = [] os.environ['TURTLEBOT3_MODEL'] = 'burger' + nav2_map = os.path.join(package_dir, 'resource', 'turtlebot3_burger_example_map.yaml') + nav2_params = os.path.join(package_dir, 'resource', 'nav2_params.yaml') if 'turtlebot3_navigation2' in get_packages_with_prefixes(): turtlebot_navigation = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join( @@ -128,34 +132,6 @@ def get_ros2_nodes(*args): nodes_to_start=navigation_nodes + ros_control_spawners ) - return [ - robot_state_publisher, - turtlebot_driver, - footprint_publisher, - waiting_nodes, - ] - - -def generate_launch_description(): - package_dir = get_package_share_directory('webots_ros2_turtlebot') - world = LaunchConfiguration('world') - mode = LaunchConfiguration('mode') - - webots = WebotsLauncher( - world=PathJoinSubstitution([package_dir, 'worlds', world]), - mode=mode, - ros2_supervisor=True - ) - - # The following line is important! - # This event handler respawns the ROS 2 nodes on simulation reset (supervisor process ends). - reset_handler = launch.actions.RegisterEventHandler( - event_handler=launch.event_handlers.OnProcessExit( - target_action=webots._supervisor, - on_exit=get_ros2_nodes, - ) - ) - return LaunchDescription([ DeclareLaunchArgument( 'world', @@ -170,19 +146,19 @@ def generate_launch_description(): webots, webots._supervisor, + robot_state_publisher, + footprint_publisher, + + turtlebot_driver, + waiting_nodes, + # This action will kill all nodes once the Webots simulation has exited launch.actions.RegisterEventHandler( event_handler=launch.event_handlers.OnProcessExit( target_action=webots, on_exit=[ - launch.actions.UnregisterEventHandler( - event_handler=reset_handler.event_handler - ), launch.actions.EmitEvent(event=launch.events.Shutdown()) ], ) ), - - # Add the reset event handler - reset_handler - ] + get_ros2_nodes()) + ]) diff --git a/webots_ros2_universal_robot/CHANGELOG.rst b/webots_ros2_universal_robot/CHANGELOG.rst index 3e33fb74f..a289bed2c 100644 --- a/webots_ros2_universal_robot/CHANGELOG.rst +++ b/webots_ros2_universal_robot/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package webots_ros2_universal_robot ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2023.1.0 (2023-XX-XX) +------------------ +* Clean simulation reset in launch file. + 2023.0.4 (2023-05-23) ------------------ * Start ros control nodes when Webots is ready. diff --git a/webots_ros2_universal_robot/launch/multirobot_launch.py b/webots_ros2_universal_robot/launch/multirobot_launch.py index 0fbbd7147..26fdaa013 100644 --- a/webots_ros2_universal_robot/launch/multirobot_launch.py +++ b/webots_ros2_universal_robot/launch/multirobot_launch.py @@ -39,9 +39,6 @@ def get_ros2_nodes(*args): package_dir = get_package_share_directory(PACKAGE_NAME) ur5e_xacro_path = os.path.join(package_dir, 'resource', 'ur5e_with_gripper.urdf.xacro') ur5e_description = xacro.process_file(ur5e_xacro_path, mappings={'name': 'UR5eWithGripper'}).toxml() - abb_description = pathlib.Path(os.path.join(package_dir, 'resource', 'webots_abb_description.urdf')).read_text() - ur5e_control_params = os.path.join(package_dir, 'resource', 'ros2_control_config.yaml') - abb_control_params = os.path.join(package_dir, 'resource', 'ros2_control_abb_config.yaml') # Define your URDF robots here # The name of an URDF robot has to match the WEBOTS_CONTROLLER_URL of the driver node @@ -55,38 +52,6 @@ def get_ros2_nodes(*args): rotation='0 0 1 -1.5708', ) - # Driver nodes - # When having multiple robot it is enough to specify the `additional_env` argument. - # The `WEBOTS_CONTROLLER_URL` has to match the robot name in the world file. - # You can check for more information at: - # https://cyberbotics.com/doc/guide/running-extern-robot-controllers#single-simulation-and-multiple-extern-robot-controllers - ur5e_driver = Node( - package='webots_ros2_driver', - executable='driver', - output='screen', - additional_env={'WEBOTS_CONTROLLER_URL': controller_url_prefix() + 'UR5e'}, - namespace='ur5e', - parameters=[ - {'robot_description': ur5e_description}, - {'use_sim_time': True}, - ur5e_control_params - ] - ) - - # Standard Webots robot using driver node - abb_driver = Node( - package='webots_ros2_driver', - executable='driver', - output='screen', - additional_env={'WEBOTS_CONTROLLER_URL': controller_url_prefix() + 'abbirb4600'}, - namespace='abb', - parameters=[ - {'robot_description': abb_description}, - {'use_sim_time': True}, - abb_control_params - ] - ) - # ROS control spawners controller_manager_timeout = ['--controller-manager-timeout', '75'] controller_manager_prefix = 'python.exe' if os.name == 'nt' else '' @@ -139,16 +104,13 @@ def get_ros2_nodes(*args): # Request to spawn the URDF robot spawn_URDF_ur5e, - # Standard Webots robot - abb_driver, - # Launch the driver node once the URDF robot is spawned. # You might include other nodes to start them with the driver node. launch.actions.RegisterEventHandler( event_handler=launch.event_handlers.OnProcessIO( target_action=spawn_URDF_ur5e, on_stdout=lambda event: get_webots_driver_node( - event, [ur5e_driver, ur5e_controller, abb_controller] + ur5e_spawners + abb_spawners + event, [ur5e_controller, abb_controller] + ur5e_spawners + abb_spawners ), ) ), @@ -165,6 +127,45 @@ def generate_launch_description(): ros2_supervisor=True ) + # Driver nodes + # When having multiple robot it is enough to specify the `additional_env` argument. + # The `WEBOTS_CONTROLLER_URL` has to match the robot name in the world file. + # You can check for more information at: + # https://cyberbotics.com/doc/guide/running-extern-robot-controllers#single-simulation-and-multiple-extern-robot-controllers + ur5e_xacro_path = os.path.join(package_dir, 'resource', 'ur5e_with_gripper.urdf.xacro') + ur5e_description = xacro.process_file(ur5e_xacro_path, mappings={'name': 'UR5eWithGripper'}).toxml() + ur5e_control_params = os.path.join(package_dir, 'resource', 'ros2_control_config.yaml') + ur5e_driver = Node( + package='webots_ros2_driver', + executable='driver', + output='screen', + additional_env={'WEBOTS_CONTROLLER_URL': controller_url_prefix() + 'UR5e'}, + namespace='ur5e', + parameters=[ + {'robot_description': ur5e_description}, + {'use_sim_time': True}, + ur5e_control_params + ], + respawn=True + ) + + # Standard Webots robot using driver node + abb_description = pathlib.Path(os.path.join(package_dir, 'resource', 'webots_abb_description.urdf')).read_text() + abb_control_params = os.path.join(package_dir, 'resource', 'ros2_control_abb_config.yaml') + abb_driver = Node( + package='webots_ros2_driver', + executable='driver', + output='screen', + additional_env={'WEBOTS_CONTROLLER_URL': controller_url_prefix() + 'abbirb4600'}, + namespace='abb', + parameters=[ + {'robot_description': abb_description}, + {'use_sim_time': True}, + abb_control_params + ], + respawn=True + ) + # The following line is important! # This event handler respawns the ROS 2 nodes on simulation reset (supervisor process ends). reset_handler = launch.actions.RegisterEventHandler( @@ -183,6 +184,9 @@ def generate_launch_description(): webots, webots._supervisor, + ur5e_driver, + abb_driver, + # This action will kill all nodes once the Webots simulation has exited launch.actions.RegisterEventHandler( event_handler=launch.event_handlers.OnProcessExit(