Cleanup simulation reset in launch files (#764)

* launch files * changelogs * restore supervisor for clock topic * update universal robot * Update CHANGELOG.rst
cyberbotics · Jun 7, 2023 · 720cb0c · 720cb0c
1 parent b901918
commit 720cb0c
Show file tree

Hide file tree

Showing 13 changed files with 199 additions and 283 deletions.
diff --git 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
@@ -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
@@ -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': '<robot name=""><link name=""/></robot>'
+        }],
+    )
+
+    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': '<robot name=""><link name=""/></robot>'
-        }],
-    )
-
-    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
@@ -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
@@ -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
@@ -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
@@ -29,49 +29,31 @@
 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',
         output='screen',
         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
@@ -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)
 ------------------