Merge pull request #769 from cyberbotics/sync-master-720cb0c

Merge master into develop
cyberbotics · Jun 8, 2023 · 3762d7a · 3762d7a
2 parents b8fe29e + 6da4618
commit 3762d7a
Show file tree

Hide file tree

Showing 15 changed files with 194 additions and 285 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.
 * Added new TIAGo project to webots_ros2_tiago to run real robot configuration.
 * Added new WebotsController node in the driver interface to launch robot controller plugins.
 * Fixed unfound robot window library in Tesla example.

diff --git a/webots_ros2_driver/webots_ros2_driver/webots_controller.py b/webots_ros2_driver/webots_ros2_driver/webots_controller.py
@@ -28,7 +28,8 @@
 
 
 class WebotsController(ExecuteProcess):
-    def __init__(self, output='screen', remappings=[], namespace='', parameters=[], robot_name='', port='1234', **kwargs):
+    def __init__(self, output='screen', respawn=False, remappings=[],
+                 namespace='', parameters=[], robot_name='', port='1234', **kwargs):
         webots_controller = (os.path.join(get_package_share_directory('webots_ros2_driver'), 'scripts', 'webots-controller'))
 
         protocol = controller_protocol()
@@ -74,6 +75,7 @@ def __init__(self, output='screen', remappings=[], namespace='', parameters=[],
                 *file_parameters,
             ],
             name=node_name,
+            respawn=respawn,
             # Set WEBOTS_HOME to package directory to load correct controller library
             additional_env={'WEBOTS_HOME': get_package_prefix('webots_ros2_driver')},
             **kwargs

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.
 * Update driver node to new WebotsController node.
 
 2023.0.4 (2023-05-23)

diff --git a/webots_ros2_epuck/launch/robot_launch.py b/webots_ros2_epuck/launch/robot_launch.py
@@ -30,17 +30,37 @@
 from webots_ros2_driver.wait_for_controller_connection import WaitForControllerConnection
 
 
-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_path = os.path.join(package_dir, 'resource', 'epuck_webots.urdf')
-    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 ''
@@ -66,6 +86,8 @@ def get_ros2_nodes(*args):
     )
     ros_control_spawners = [diffdrive_controller_spawner, joint_state_broadcaster_spawner]
 
+    robot_description_path = os.path.join(package_dir, 'resource', 'epuck_webots.urdf')
+    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 = WebotsController(
         robot_name='e-puck',
@@ -75,7 +97,8 @@ def get_ros2_nodes(*args):
              'set_robot_state_publisher': True},
             ros2_control_params
         ],
-        remappings=mappings
+        remappings=mappings,
+        respawn=True
     )
 
     epuck_process = Node(
@@ -87,22 +110,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(
@@ -124,33 +131,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',
@@ -160,19 +140,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
@@ -4,6 +4,7 @@ Changelog for package webots_ros2_mavic
 
 2023.1.0 (2023-XX-XX)
 ------------------
+* Clean simulation reset in launch file.
 * Update driver node to new WebotsController node.
 
 2023.0.2 (2023-02-07)

diff --git a/webots_ros2_mavic/launch/robot_launch.py b/webots_ros2_mavic/launch/robot_launch.py
@@ -27,22 +27,6 @@
 from webots_ros2_driver.webots_controller import WebotsController
 
 
-def get_ros2_nodes(*args):
-    package_dir = get_package_share_directory('webots_ros2_mavic')
-    robot_description_path = os.path.join(package_dir, 'resource', 'mavic_webots.urdf')
-
-    mavic_driver = WebotsController(
-        robot_name='Mavic_2_PRO',
-        parameters=[
-            {'robot_description': robot_description_path},
-        ]
-    )
-
-    return [
-        mavic_driver,
-    ]
-
-
 def generate_launch_description():
     package_dir = get_package_share_directory('webots_ros2_mavic')
     world = LaunchConfiguration('world')
@@ -52,13 +36,13 @@ 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_path = os.path.join(package_dir, 'resource', 'mavic_webots.urdf')
+    mavic_driver = WebotsController(
+        robot_name='Mavic_2_PRO',
+        parameters=[
+            {'robot_description': robot_description_path},
+        ],
+        respawn=True
     )
 
     return LaunchDescription([
@@ -69,20 +53,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
@@ -4,6 +4,7 @@ Changelog for package webots_ros2_tesla
 
 2023.1.0 (2023-XX-XX)
 ------------------
+* Clean simulation reset in launch file.
 * Update driver node to new WebotsController node.
 
 2023.0.2 (2023-02-07)

diff --git a/webots_ros2_tesla/launch/robot_launch.py b/webots_ros2_tesla/launch/robot_launch.py
@@ -28,46 +28,28 @@
 from webots_ros2_driver.webots_controller import WebotsController
 
 
-def get_ros2_nodes(*args):
+def generate_launch_description():
     package_dir = get_package_share_directory('webots_ros2_tesla')
-    robot_description_path = os.path.join(package_dir, 'resource', 'tesla_webots.urdf')
+    world = LaunchConfiguration('world')
+
+    webots = WebotsLauncher(
+        world=PathJoinSubstitution([package_dir, 'worlds', world]),
+        ros2_supervisor=True
+    )
 
+    robot_description_path = os.path.join(package_dir, 'resource', 'tesla_webots.urdf')
     tesla_driver = WebotsController(
         robot_name='vehicle',
         parameters=[
             {'robot_description': robot_description_path}
-        ]
+        ],
+        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',
@@ -76,20 +58,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.
 * Added new world, resources and launch file to start the TIAGo with real robot configuration.
 * Update driver node to new WebotsController node.