From 52a52d09928ce8f1e159a667bf6e8a0e125ccf05 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Wed, 12 Oct 2022 16:09:19 +0200 Subject: [PATCH 1/6] Update intro to WSL --- source/Tutorials/Advanced/Simulators/Webots.rst | 16 ++++------------ 1 file changed, 4 insertions(+), 12 deletions(-) diff --git a/source/Tutorials/Advanced/Simulators/Webots.rst b/source/Tutorials/Advanced/Simulators/Webots.rst index 13c53714eb4..e5fdb4ef00c 100644 --- a/source/Tutorials/Advanced/Simulators/Webots.rst +++ b/source/Tutorials/Advanced/Simulators/Webots.rst @@ -35,7 +35,7 @@ Prerequisites It is recommended to understand basic ROS principles covered in the beginner :doc:`../../../Tutorials`. In particular, :doc:`../../Beginner-CLI-Tools/Introducing-Turtlesim/Introducing-Turtlesim`, :doc:`../../Beginner-CLI-Tools/Understanding-ROS2-Topics/Understanding-ROS2-Topics`, :doc:`../../Beginner-Client-Libraries/Creating-A-Workspace/Creating-A-Workspace`, :doc:`../../Beginner-Client-Libraries/Creating-Your-First-ROS2-Package` and :doc:`../../Intermediate/Launch/Creating-Launch-Files` are useful prerequisites. -Finally, you will need to install ``webots_ros2_driver`` from a terminal with this command: +Finally, you will need to install ``webots_ros2_driver`` from a terminal with the following commands. On Windows, a WSL environment should be configured. These `instructions `_ explain how to setup such installation. .. tabs:: @@ -50,17 +50,9 @@ Finally, you will need to install ``webots_ros2_driver`` from a terminal with th .. code-block:: console - # Install webots_ros2_driver and dependencies - cd \ros2_ws - pip install rosinstall_generator - rosinstall_generator webots_ros2_driver --deps --exclude-path=C:\dev\ros2_{DISTRO} > deps.repos - vcs import src < deps.repos - - # Build the packages - colcon build - - # Source this workspace - call install\local_setup.bat + # Install webots_ros2_driver in WSL + sudo apt update + sudo apt install ros-{DISTRO}-webots-ros2-driver .. note:: From 883aceb138f19ffd7edf63c3b12d77350820a979 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Wed, 12 Oct 2022 16:21:05 +0200 Subject: [PATCH 2/6] Revert "Update intro to WSL" This reverts commit 52a52d09928ce8f1e159a667bf6e8a0e125ccf05. --- source/Tutorials/Advanced/Simulators/Webots.rst | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/source/Tutorials/Advanced/Simulators/Webots.rst b/source/Tutorials/Advanced/Simulators/Webots.rst index e5fdb4ef00c..13c53714eb4 100644 --- a/source/Tutorials/Advanced/Simulators/Webots.rst +++ b/source/Tutorials/Advanced/Simulators/Webots.rst @@ -35,7 +35,7 @@ Prerequisites It is recommended to understand basic ROS principles covered in the beginner :doc:`../../../Tutorials`. In particular, :doc:`../../Beginner-CLI-Tools/Introducing-Turtlesim/Introducing-Turtlesim`, :doc:`../../Beginner-CLI-Tools/Understanding-ROS2-Topics/Understanding-ROS2-Topics`, :doc:`../../Beginner-Client-Libraries/Creating-A-Workspace/Creating-A-Workspace`, :doc:`../../Beginner-Client-Libraries/Creating-Your-First-ROS2-Package` and :doc:`../../Intermediate/Launch/Creating-Launch-Files` are useful prerequisites. -Finally, you will need to install ``webots_ros2_driver`` from a terminal with the following commands. On Windows, a WSL environment should be configured. These `instructions `_ explain how to setup such installation. +Finally, you will need to install ``webots_ros2_driver`` from a terminal with this command: .. tabs:: @@ -50,9 +50,17 @@ Finally, you will need to install ``webots_ros2_driver`` from a terminal with th .. code-block:: console - # Install webots_ros2_driver in WSL - sudo apt update - sudo apt install ros-{DISTRO}-webots-ros2-driver + # Install webots_ros2_driver and dependencies + cd \ros2_ws + pip install rosinstall_generator + rosinstall_generator webots_ros2_driver --deps --exclude-path=C:\dev\ros2_{DISTRO} > deps.repos + vcs import src < deps.repos + + # Build the packages + colcon build + + # Source this workspace + call install\local_setup.bat .. note:: From bfc15537282018df9f4ecd9593e55f898597712b Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Thu, 13 Oct 2022 14:19:44 +0200 Subject: [PATCH 3/6] Update world with declarations --- source/Tutorials/Advanced/Simulators/Code/my_world.wbt | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) 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 { From 8b25cdbeddca859441ba7a1645edf77521f00b5f Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Thu, 13 Oct 2022 15:03:03 +0200 Subject: [PATCH 4/6] Add supervisor node and environment var --- source/Tutorials/Advanced/Simulators/Code/robot_launch.py | 6 +++++- .../Advanced/Simulators/Code/robot_launch_sensor.py | 6 +++++- 2 files changed, 10 insertions(+), 2 deletions(-) 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( From 1a352be49ffaaaacc041334ab43e6696b8bf4421 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Thu, 13 Oct 2022 16:02:46 +0200 Subject: [PATCH 5/6] Update tutorial with explanations --- .../Tutorials/Advanced/Simulators/Webots.rst | 20 ++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/source/Tutorials/Advanced/Simulators/Webots.rst b/source/Tutorials/Advanced/Simulators/Webots.rst index 13c53714eb4..b87a7b6d57c 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 the ``webots`` node. +It allows to spawn the needed objects and create useful topics. +The ``Ros2Supervisor`` node must be started, using the ``Ros2SupervisorLauncher``, to communicate with this Robot. + +.. 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``. From c8549cb10fa717d2a20fd10bcc6f08ccb0bfe6e0 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz <61198661+ygoumaz@users.noreply.github.com> Date: Thu, 13 Oct 2022 16:31:17 +0200 Subject: [PATCH 6/6] Update source/Tutorials/Advanced/Simulators/Webots.rst Co-authored-by: ad-daniel <44834743+ad-daniel@users.noreply.github.com> --- source/Tutorials/Advanced/Simulators/Webots.rst | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/source/Tutorials/Advanced/Simulators/Webots.rst b/source/Tutorials/Advanced/Simulators/Webots.rst index b87a7b6d57c..fb6afbe36f6 100644 --- a/source/Tutorials/Advanced/Simulators/Webots.rst +++ b/source/Tutorials/Advanced/Simulators/Webots.rst @@ -204,9 +204,9 @@ 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 the ``webots`` node. -It allows to spawn the needed objects and create useful topics. -The ``Ros2Supervisor`` node must be started, using the ``Ros2SupervisorLauncher``, to communicate with this Robot. +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