Tutorial Webots Simulation Reset with ROS 2 - cyberbotics/webots_ros2 GitHub Wiki

The Webots reset button reverts the world to the initial state and restarts controllers. It is convenient as it quickly resets the simulation, but in the context of ROS 2, robot controllers are not started again making the simulation stop. The controllers cannot be started as Webots is not allowed to start extern controllers (a driver node from the webots_ros2_driver package in the context of ROS) spawned in a ROS 2 launch file.

Therefore, restarting of the driver node has to be handled manually, as in the generate_launch_description function below:

def generate_launch_description():
    webots_driver = Node(
        package='webots_ros2_driver',
        executable='driver',
        parameters=[
            {'robot_description': robot_description,
             'set_robot_state_publisher': True }
        ],

        # The following line is important!
        # Every time one resets the simulation the controller is automatically respawned
        respawn=True
    )

    # Starts Webots
    webots = WebotsLauncher(world=PathJoinSubstitution([package_dir, 'worlds', world]))

    # Starts the Ros2Supervisor node, with by default respawn=True
    ros2_supervisor = Ros2SupervisorLauncher() # Only with the develop branch!

    return LaunchDescription([
        webots,
        ros2_supervisor, # Only with the develop branch!
        webots_driver,
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=webots,
                on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())],
            )
        )
    ])

On the reset, Webots kills all driver nodes. Therefore, to start them again after the reset is done, you can set the respawn property of the driver node to True. It will ensure driver nodes are up and running after the reset.

If you have some other nodes that have to be restarted (e.g. ros2_control) then you can use the OnProcessExit event handler:

def get_ros2_control_spawners(*args):
    return [
        Node(
            package='controller_manager',
            executable='spawner.py',
            arguments=['diffdrive_controller']
        )
    ]

def generate_launch_description():
    webots_driver = Node(
        package='webots_ros2_driver',
        executable='driver',
        parameters=[
            {'robot_description': robot_description,
             'set_robot_state_publisher': True }
        ],

        # The following line is important!
        # Every time one resets the simulation the controller is automatically respawned
        respawn=True
    )

    # Starts Webots
    webots = WebotsLauncher(world=PathJoinSubstitution([package_dir, 'worlds', world]))

    # Starts the Ros2Supervisor node, with by default respawn=True
    ros2_supervisor = Ros2SupervisorLauncher() # Only with the develop branch!

    return LaunchDescription([
        webots,
        ros2_supervisor, # Only with the develop branch!
        webots_driver,
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=webots,
                on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())],
            )
        ),

        # The following line is important!
        # This event respawns ros2_controllers on the `driver` node shutdown.
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=webots_driver,
                on_exit=get_ros2_control_spawners,
            )
        )
    ] + get_ros2_control_spawners())