Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 6 additions & 1 deletion source/Tutorials/Advanced/Simulators/Code/my_world.wbt
Original file line number Diff line number Diff line change
@@ -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 {
Expand Down
6 changes: 5 additions & 1 deletion source/Tutorials/Advanced/Simulators/Code/robot_launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -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():
Expand All @@ -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},
]
Expand All @@ -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,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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():
Expand All @@ -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},
]
Expand All @@ -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(
Expand Down
20 changes: 15 additions & 5 deletions source/Tutorials/Advanced/Simulators/Webots.rst
Original file line number Diff line number Diff line change
Expand Up @@ -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
^^^^^^^^^^^^^^^^^^^^^^^^^^
Expand Down Expand Up @@ -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``.

Expand Down