Tutorial Ros2Supervisor Refresh or Add a Urdf Robot - cyberbotics/webots_ros2 GitHub Wiki

⚠️THE SUPPORT FOR URDF AND XACRO IS DEPRECATED! IT WILL BE REMOVED IN VERSION 2024.0.0.⚠️

Importing URDF robot in simulation is an easy task with the Ros2Supervisor node, but it might be convenient to be able to refresh the robot inside a running simulation rather than having to stop the simulation and relaunch it.

In this tutorial you will learn how to refresh your URDF robot without stopping your simulation. It can also be used to add URDF robots at a certain moment in the simulation and remove them at another moment.

Notes: If you have not yet read the tutorial on how to Import a URDF robot in your simulation, make sure to do it as a certain number of notions will be used in this tutorial without being re-explained.

Set up

The set up is close to the one for importing a URDF robot, with the exception that the launch file will be split in two files.

The first one will start Webots and the Ros2Supervisor node while the second one will do the request to spawn the robot and start the webots_ros2_driver node.

The idea is to start the simulation with the first launch file, use the second launch file to spawn the URDF robot and when a refresh of the robot is needed, stop and relaunch the second launch file. In case you want to use other ROS 2 nodes, they should be started from the second launch file.

With this method, Webots will not reload the world, saving a bit of your time.

Launch file templates

Here is how to setup the first launch file:

import launch
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch.actions import DeclareLaunchArgument
from launch.substitutions.path_join_substitution import PathJoinSubstitution
from ament_index_python.packages import get_package_share_directory
from webots_ros2_driver.webots_launcher import WebotsLauncher, Ros2SupervisorLauncher

def generate_launch_description():
    package_dir = get_package_share_directory('my_package')
    world = LaunchConfiguration('world')

    # The WebotsLauncher is a Webots custom action that allows you to start a Webots simulation instance.
    # It searches for the Webots installation in the path specified by the `WEBOTS_HOME` environment variable and default installation paths.
    # The accepted arguments are:
    # - `world` (str): Path to the world to launch.
    # - `gui` (bool): Whether to display GUI or not.
    # - `mode` (str): Can be `pause`, `realtime`, or `fast`.
    webots = WebotsLauncher(
        world=PathJoinSubstitution([package_dir, 'worlds', world])
    )

    # The Ros2Supervisor is a Webots custom node that communicates with a Supervisor robot in the simulation.
    # The accepted arguments from launch.actions.ExecuteProcess are:
    # - `output` (string): Output configuration for process output logging. Default is 'screen'.
    # - `respawn` (bool): Relaunch the process that abnormally died.  Default is 'True'.
    ros2_supervisor = Ros2SupervisorLauncher()

    return LaunchDescription([
        DeclareLaunchArgument(
            'world',
            default_value='my_world.wbt',
            description='Choose one of the world files from `/my_package/worlds` directory'
        ),
        webots,
        ros2_supervisor,
        # 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.EmitEvent(event=launch.events.Shutdown())],
            )
        )
    ])

The second launcher will request the Ros2Supervisor node to spawn your URDF robot and start the webots_ros2_driver node when the robot has been successfully spawned:

import os
import pathlib
import launch
from launch_ros.actions import Node
from launch import LaunchDescription
from ament_index_python.packages import get_package_share_directory
from webots_ros2_driver.urdf_spawner import URDFSpawner, get_webots_driver_node

def generate_launch_description():
    package_dir = get_package_share_directory('my_package')
    urdf_path = os.path.join(package_dir, 'resource', 'my_urdf_file.urdf')
    robot_description = pathlib.Path(urdf_path).read_text()

    # Define your URDF robots here
    # The name of an URDF robot has to match the WEBOTS_ROBOT_NAME of the driver node
    spawn_URDF_ur5e = URDFSpawner(
        name='myRobot',
        urdf_path=urdf_path,
        translation='0 0 1',
        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
    universal_robot_driver = Node(
        package='webots_ros2_driver',
        executable='driver',
        output='screen',
        additional_env={'WEBOTS_CONTROLLER_URL': 'myRobot'},
        parameters=[
            {'robot_description': robot_description},
        ],
    )

    return LaunchDescription([
        # Request to spawn the URDF robot
        spawn_URDF_ur5e,

        # Launch the driver node once the URDF robot is spawned
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessIO(
                target_action=spawn_URDF_ur5e,
                on_stdout=lambda event: get_webots_driver_node(event, universal_robot_driver),
            )
        ),

        # Kill all the nodes when the driver node is shut down (useful with other ROS 2 nodes)
        launch.actions.RegisterEventHandler(
                event_handler=launch.event_handlers.OnProcessExit(
                    target_action=universal_robot_driver,
                    on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())],
                )
            ),
    ])

Example

An example of the first launch file can be found here and an example of the second launch file can be found here.

This launch file is the one used for the UR5e Simple Example example from Universal Robots.

You can test these two launch files as they are shipped with the webots_ros2_universal_robot package by first running the following in a terminal:

ros2 launch webots_ros2_universal_robot robot_world_launch.py

As you can see, Webots will be launched but the robot is not yet here. Open a second terminal and run:

ros2 launch webots_ros2_universal_robot robot_nodes_launch.py

The UR5e robot is now here!

You can interrupt the second terminal or refresh the simulation in order to remove the robot and then rerun the command to re-spawn it.

In a third terminal you can verify the functionalities of this example by running:

ros2 action send_goal /ur_joint_trajectory_controller/follow_joint_trajectory control_msgs/action/FollowJointTrajectory "{
  trajectory: {
    joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint],
    points: [
      { positions: [3.02, -1.63, -1.88, 1.01, 1.51, 1.13], time_from_start: { sec: 5, nanosec: 500 } },
      { positions: [-1.01, 0.38, -0.63, -0.88, 0.25, -1.63], time_from_start: { sec: 6, nanosec: 500 } },
      { positions: [-1.01, 0.38, -0.63, -0.88, 0.25, 6.2], time_from_start: { sec: 50, nanosec: 500 } }
    ]
  },
  goal_tolerance: [
    { name: shoulder_pan_joint, position: 0.01 },
    { name: shoulder_lift_joint, position: 0.01 },
    { name: elbow_joint, position: 0.01 },
    { name: wrist_1_joint, position: 0.01 },
    { name: wrist_2_joint, position: 0.01 },
    { name: wrist_3_joint, position: 0.01 }
  ]
}"