diff --git a/source/Tutorials/Advanced/Simulators/Code/my_world.wbt b/source/Tutorials/Advanced/Simulators/Code/my_world.wbt index 909f6b8dcf9..76f85355629 100644 --- a/source/Tutorials/Advanced/Simulators/Code/my_world.wbt +++ b/source/Tutorials/Advanced/Simulators/Code/my_world.wbt @@ -1,4 +1,9 @@ -#VRML_SIM R2022a utf8 +#VRML_SIM R2022b utf8 + +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/backgrounds/protos/TexturedBackground.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/floors/protos/CircleArena.proto" + WorldInfo { } Viewpoint { diff --git a/source/Tutorials/Advanced/Simulators/Code/robot_launch.py b/source/Tutorials/Advanced/Simulators/Code/robot_launch.py index bc708497936..269b711f3c7 100644 --- a/source/Tutorials/Advanced/Simulators/Code/robot_launch.py +++ b/source/Tutorials/Advanced/Simulators/Code/robot_launch.py @@ -4,7 +4,7 @@ from launch_ros.actions import Node from launch import LaunchDescription from ament_index_python.packages import get_package_share_directory -from webots_ros2_driver.webots_launcher import WebotsLauncher +from webots_ros2_driver.webots_launcher import WebotsLauncher, Ros2SupervisorLauncher def generate_launch_description(): @@ -15,10 +15,13 @@ def generate_launch_description(): world=os.path.join(package_dir, 'worlds', 'my_world.wbt') ) + ros2_supervisor = Ros2SupervisorLauncher() + my_robot_driver = Node( package='webots_ros2_driver', executable='driver', output='screen', + additional_env={'WEBOTS_CONTROLLER_URL': 'my_robot'}, parameters=[ {'robot_description': robot_description}, ] @@ -27,6 +30,7 @@ def generate_launch_description(): return LaunchDescription([ webots, my_robot_driver, + ros2_supervisor, launch.actions.RegisterEventHandler( event_handler=launch.event_handlers.OnProcessExit( target_action=webots, diff --git a/source/Tutorials/Advanced/Simulators/Code/robot_launch_sensor.py b/source/Tutorials/Advanced/Simulators/Code/robot_launch_sensor.py index a3aa9868dbb..9bc31f74a29 100644 --- a/source/Tutorials/Advanced/Simulators/Code/robot_launch_sensor.py +++ b/source/Tutorials/Advanced/Simulators/Code/robot_launch_sensor.py @@ -4,7 +4,7 @@ from launch_ros.actions import Node from launch import LaunchDescription from ament_index_python.packages import get_package_share_directory -from webots_ros2_driver.webots_launcher import WebotsLauncher +from webots_ros2_driver.webots_launcher import WebotsLauncher, Ros2SupervisorLauncher def generate_launch_description(): @@ -15,10 +15,13 @@ def generate_launch_description(): world=os.path.join(package_dir, 'worlds', 'my_world.wbt') ) + ros2_supervisor = Ros2SupervisorLauncher() + my_robot_driver = Node( package='webots_ros2_driver', executable='driver', output='screen', + additional_env={'WEBOTS_CONTROLLER_URL': 'my_robot'}, parameters=[ {'robot_description': robot_description}, ] @@ -32,6 +35,7 @@ def generate_launch_description(): return LaunchDescription([ webots, my_robot_driver, + ros2_supervisor, obstacle_avoider, launch.actions.RegisterEventHandler( event_handler=launch.event_handlers.OnProcessExit( diff --git a/source/Tutorials/Advanced/Simulators/Webots.rst b/source/Tutorials/Advanced/Simulators/Webots.rst index 13c53714eb4..fb6afbe36f6 100644 --- a/source/Tutorials/Advanced/Simulators/Webots.rst +++ b/source/Tutorials/Advanced/Simulators/Webots.rst @@ -204,31 +204,41 @@ You have to specify in the constructor which world file the simulator will open. :dedent: 4 :lines: 14-16 +A supervisor Robot is always automatically added to the world file by ``WebotsLauncher``. +This robot is controlled by the custom node ``Ros2Supervisor``, which must also be started using the ``Ros2SupervisorLauncher``. +This node allows to spawn URDF robots directly into the world, and it also publishes useful topics like ``/clock``. + +.. literalinclude:: Code/robot_launch.py + :language: python + :dedent: 4 + :lines: 18 + Then, the ROS node interacting with the simulated robot is created. This node, named ``driver``, is located in the ``webots_ros2_driver`` package. The node will be able to communicate with the simulated robot by using a custom protocol based on IPC and shared memory. In your case, you need to run a single instance of this node, because you have a single robot in the simulation. But if you had more robots in the simulation, you would have to run one instance of this node per robot. +``WEBOTS_CONTROLLER_URL`` is used to define the name of the robot the driver should connect to. The ``robot_description`` parameter holds the contents of the URDF file which refers to the ``my_robot_driver.py`` Python plugin. .. literalinclude:: Code/robot_launch.py :language: python :dedent: 4 - :lines: 18-25 + :lines: 20-28 -After that, the two nodes are set to be launched in the ``LaunchDescription`` constructor: +After that, the three nodes are set to be launched in the ``LaunchDescription`` constructor: .. literalinclude:: Code/robot_launch.py :language: python :dedent: 4 - :lines: 27-29 + :lines: 30-33 Finally, an optional part is added in order to shutdown all the nodes once Webots terminates (e.g., when it gets closed from the graphical user interface). .. literalinclude:: Code/robot_launch.py :language: python :dedent: 8 - :lines: 30-35 + :lines: 34-39 6 Modify the setup.py file ^^^^^^^^^^^^^^^^^^^^^^^^^^ @@ -351,7 +361,7 @@ Go to the file ``robot_launch.py`` and replace ``def generate_launch_description .. literalinclude:: Code/robot_launch_sensor.py :language: python - :lines: 10-42 + :lines: 10-46 This will create an ``obstacle_avoider`` node that will be included in the ``LaunchDescription``.