Sync master with develop (#630)

* Enforce clang format (#599) * Apply clangformat * Add test sources * Test pep8 * Fix workflow * Fix workflow * Fix script * pep8 fixes * Cpp fixes * Remove ament pep257 & flake8 * Update urdf2webots submodule * Revert "Update urdf2webots submodule" This reverts commit fa45543. * Revert "Remove ament pep257 & flake8" This reverts commit 92df8a3. * Revert "Cpp fixes" This reverts commit c3a497c. * Revert "pep8 fixes" This reverts commit 141b0c8. * Revert "Revert "pep8 fixes"" This reverts commit ad007a4. * Test * Revert "Test" This reverts commit 25934ef. * Revert "Revert "Cpp fixes"" This reverts commit 8462d81. * Revert "Revert "Remove ament pep257 & flake8"" This reverts commit c689352. * Revert "Revert "Update urdf2webots submodule"" This reverts commit e2ed2b3. * Order fixes * Skip submodule tests * Finalize * Sync submodule (#602) Co-authored-by: ad-daniel <[email protected]> * Sync submodule (#606) Co-authored-by: ad-daniel <[email protected]> * Sync submodule (#609) Co-authored-by: ad-daniel <[email protected]> * Sync submodule (#613) Co-authored-by: ad-daniel <[email protected]> * Fix macOS local assets with relative URLs (#612) * Add macOS copy * remove host * Update changelogs * Optional Ros2 Supervisor (#616) * webots launcher creates supervisor if needed * Update launch files * minor fix * Update changelogs * Fix URDF spawner on macOS and WSL (#610) * add wsl and macos urdfpath edit * Better path manipulation in supervisor * fix package names in yalm and xacro * Fix resources copy and prefix * Fix directory split * Update wsl * fix wsl * Update changelogs * cleanup * fix sources * Update webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py Co-authored-by: ad-daniel <[email protected]> * Update ros2_supervisor.py * clear trailing whitespace * Update changelog --------- Co-authored-by: ad-daniel <[email protected]> * update changelogs and version (#620) * Sync submodule (#623) Co-authored-by: ad-daniel <[email protected]> * adding possibility to select stream type (#625) * adding possibility to select stream type * Update webots_ros2_driver/webots_ros2_driver/webots_launcher.py Co-authored-by: Olivier Michel <[email protected]> --------- Co-authored-by: Sebastian Ohl <[email protected]> Co-authored-by: Olivier Michel <[email protected]> * Fix tests (#633) * fix pep8 * fix pep8 --------- Co-authored-by: ad-daniel <[email protected]> Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> Co-authored-by: ad-daniel <[email protected]> Co-authored-by: Sebastian Ohl <[email protected]> Co-authored-by: Sebastian Ohl <[email protected]> Co-authored-by: Olivier Michel <[email protected]>
cyberbotics · Feb 17, 2023 · d999fa6 · d999fa6
1 parent 732f534
commit d999fa6
Show file tree

Hide file tree

Showing 43 changed files with 219 additions and 137 deletions.
diff --git a/webots_ros2/CHANGELOG.rst b/webots_ros2/CHANGELOG.rst
@@ -2,9 +2,12 @@
 Changelog for package webots_ros2
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-2023.0.2 (2023-XX-XX)
+2023.0.2 (2023-02-07)
 ------------------
 * Drop support for Galactic.
+* Fixed the spawn of URDF robots in WSL and macOS when using full path.
+* Fixed relative assets in macOS.
+* Ros2Supervisor is now optional.
 
 2023.0.1 (2023-01-05)
 ------------------

diff --git a/webots_ros2/package.xml b/webots_ros2/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>webots_ros2</name>
-  <version>2023.0.1</version>
+  <version>2023.0.2</version>
   <description>Interface between Webots and ROS2</description>
 
   <maintainer email="[email protected]">Cyberbotics</maintainer>

diff --git a/webots_ros2/setup.py b/webots_ros2/setup.py
@@ -6,7 +6,7 @@
 
 setup(
     name=package_name,
-    version='2023.0.1',
+    version='2023.0.2',
     packages=[package_name],
     data_files=[
         ('share/' + package_name, ['package.xml']),

diff --git a/webots_ros2_control/package.xml b/webots_ros2_control/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>webots_ros2_control</name>
-  <version>2023.0.1</version>
+  <version>2023.0.2</version>
   <description>ros2_control plugin for Webots</description>
   <maintainer email="[email protected]">Cyberbotics</maintainer>
   <url type="website">http://wiki.ros.org/webots_ros2</url>

diff --git a/webots_ros2_driver/CHANGELOG.rst b/webots_ros2_driver/CHANGELOG.rst
@@ -2,9 +2,15 @@
 Changelog for package webots_ros2_driver
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2023.0.2 (2023-02-07)
+------------------
+* Fixed the spawn of URDF robots in WSL and macOS when using full path.
+* Fixed relative assets in macOS.
+* Added Ros2Supervisor creation.
+
 2023.0.1 (2023-01-05)
 ------------------
-* Fix relative assets in WSL.
+* Fixed relative assets in WSL.
 
 2023.0.0 (2022-11-30)
 ------------------

diff --git a/webots_ros2_driver/package.xml b/webots_ros2_driver/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>webots_ros2_driver</name>
-  <version>2023.0.1</version>
+  <version>2023.0.2</version>
   <description>Implementation of the Webots - ROS 2 interface</description>
   <maintainer email="[email protected]">Cyberbotics</maintainer>
   <license>Apache License 2.0</license>

diff --git a/webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py b/webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py
@@ -19,8 +19,10 @@
 
 
 import os
-import sys
 import re
+import shutil
+import subprocess
+import sys
 
 import rclpy
 import vehicle
@@ -32,6 +34,7 @@
 from rclpy.qos import qos_profile_services_default
 from rosgraph_msgs.msg import Clock
 from std_msgs.msg import String
+from webots_ros2_driver.utils import is_wsl, has_shared_folder, container_shared_folder, host_shared_folder
 sys.path.insert(1, os.path.join(os.path.dirname(webots_ros2_importer.__file__), 'urdf2webots'))
 from urdf2webots.importer import convertUrdfFile, convertUrdfContent  # noqa
 from webots_ros2_msgs.srv import SpawnUrdfRobot, SpawnNodeFromString  # noqa
@@ -87,13 +90,78 @@ def __spawn_urdf_robot_callback(self, request, response):
         box_collision = robot.box_collision if robot.box_collision else False
         init_pos = robot.init_pos if robot.init_pos else None
 
-        # Choose the conversion according to the input
+        # Choose the conversion according to the input and platform
         if robot.urdf_path:
-            robot_string = convertUrdfFile(input=robot.urdf_path, robotName=robot_name, normal=normal,
-                                           boxCollision=box_collision, initTranslation=robot_translation,
-                                           initRotation=robot_rotation, initPos=init_pos)
+            if has_shared_folder() or is_wsl():
+                # Check that the file exists and is an URDF
+                if not os.path.isfile(robot.urdf_path):
+                    sys.exit('Input file "%s" does not exist.' % robot.urdf_path)
+                if not robot.urdf_path.endswith('.urdf'):
+                    sys.exit('"%s" is not a URDF file.' % robot.urdf_path)
+
+                # Read the content of the URDF
+                with open(robot.urdf_path, 'r') as file:
+                    urdfContent = file.read()
+                    if urdfContent is None:
+                        sys.exit('Could not read the URDF file.')
+
+                # Get the package name and parent resource directory from URDF path
+                split_path = robot.urdf_path.split(os.path.sep)
+                for i, folder in (list(enumerate(split_path))):
+                    if folder == "share":
+                        package_dir = os.path.sep.join(split_path[:i + 2])
+                        resource_dir = os.path.sep.join(split_path[:i + 3])
+                        break
+                # On macOS, the resources are copied to shared_folder/package_name/resource_folder
+                # The path prefix is updated to the path of the shared folder
+                if has_shared_folder():
+                    shared_package_dir = os.path.join(container_shared_folder(), os.path.basename(package_dir))
+                    shared_resource_dir = os.path.join(shared_package_dir, os.path.basename(resource_dir))
+                    if (not os.path.isdir(shared_package_dir)):
+                        os.mkdir(shared_package_dir)
+                    if (not os.path.isdir(shared_resource_dir)):
+                        shutil.copytree(resource_dir, shared_resource_dir)
+                    relative_path_prefix = os.path.join(host_shared_folder(), os.path.basename(package_dir),
+                                                        os.path.basename(resource_dir))
+                # In WSL, the prefix must be converted to WSL path to work in Webots running on native Windows
+                if is_wsl():
+                    relative_path_prefix = resource_dir
+                    command = ['wslpath', '-w', relative_path_prefix]
+                    relative_path_prefix = subprocess.check_output(command).strip().decode('utf-8').replace('\\', '/')
+
+                robot_string = convertUrdfContent(input=urdfContent, robotName=robot_name, normal=normal,
+                                                  boxCollision=box_collision, initTranslation=robot_translation,
+                                                  initRotation=robot_rotation, initPos=init_pos,
+                                                  relativePathPrefix=relative_path_prefix)
+            else:
+                robot_string = convertUrdfFile(input=robot.urdf_path, robotName=robot_name, normal=normal,
+                                               boxCollision=box_collision, initTranslation=robot_translation,
+                                               initRotation=robot_rotation, initPos=init_pos)
+
         elif robot.robot_description:
             relative_path_prefix = robot.relative_path_prefix if robot.relative_path_prefix else None
+            # In WSL, the prefix must be converted to WSL path to work in Webots running on native Windows
+            if is_wsl() and relative_path_prefix:
+                command = ['wslpath', '-w', relative_path_prefix]
+                relative_path_prefix = subprocess.check_output(command).strip().decode('utf-8').replace('\\', '/')
+            if has_shared_folder() and relative_path_prefix:
+                # Get the package name and parent resource directory from URDF path
+                split_path = relative_path_prefix.split(os.path.sep)
+                for i, folder in (list(enumerate(split_path))):
+                    if folder == "share":
+                        package_dir = os.path.sep.join(split_path[:i + 2])
+                        resource_dir = os.path.sep.join(split_path[:i + 3])
+                        break
+                # On macOS, the resources are copied to shared_folder/package_name/resource_folder
+                # The path prefix is updated to the path of the shared folder
+                shared_package_dir = os.path.join(container_shared_folder(), os.path.basename(package_dir))
+                shared_resource_dir = os.path.join(shared_package_dir, os.path.basename(resource_dir))
+                if (not os.path.isdir(shared_package_dir)):
+                    os.mkdir(shared_package_dir)
+                if (not os.path.isdir(shared_resource_dir)):
+                    shutil.copytree(resource_dir, shared_resource_dir)
+                relative_path_prefix = os.path.join(host_shared_folder(), os.path.basename(package_dir),
+                                                    os.path.basename(resource_dir))
             robot_string = convertUrdfContent(input=robot.robot_description, robotName=robot_name, normal=normal,
                                               boxCollision=box_collision, initTranslation=robot_translation,
                                               initRotation=robot_rotation, initPos=init_pos,

diff --git a/webots_ros2_driver/webots_ros2_driver/urdf_spawner.py b/webots_ros2_driver/webots_ros2_driver/urdf_spawner.py
@@ -16,12 +16,7 @@
 
 """This process simply sends urdf information to the Spawner through a service."""
 
-import os
-import shutil
-import subprocess
-
 from launch.actions import ExecuteProcess
-from webots_ros2_driver.utils import is_wsl, has_shared_folder, container_shared_folder, host_shared_folder
 
 
 def get_webots_driver_node(event, driver_node):
@@ -35,15 +30,6 @@ def get_webots_driver_node(event, driver_node):
 class URDFSpawner(ExecuteProcess):
     def __init__(self, output='log', name=None, urdf_path=None, robot_description=None, relative_path_prefix=None,
                  translation='0 0 0', rotation='0 0 1 0', normal=False, box_collision=False, init_pos=None, **kwargs):
-        if is_wsl() and relative_path_prefix:
-            command = ['wslpath', '-w', relative_path_prefix]
-            relative_path_prefix = subprocess.check_output(command).strip().decode('utf-8').replace('\\', '/')
-        if has_shared_folder() and relative_path_prefix and not os.path.isdir(
-          os.path.join(container_shared_folder(), os.path.basename(relative_path_prefix))):
-            shutil.copytree(relative_path_prefix, os.path.join(container_shared_folder(),
-                                                               os.path.basename(relative_path_prefix)))
-            relative_path_prefix = os.path.join(host_shared_folder(), os.path.basename(relative_path_prefix))
-
         message = '{robot: {'
 
         if name:
@@ -70,14 +56,12 @@ def __init__(self, output='log', name=None, urdf_path=None, robot_description=No
 
         message += '} }'
 
-        command = [
-                'ros2',
-                'service',
-                'call',
-                '/spawn_urdf_robot',
-                'webots_ros2_msgs/srv/SpawnUrdfRobot',
-                message
-            ]
+        command = ['ros2',
+                   'service',
+                   'call',
+                   '/spawn_urdf_robot',
+                   'webots_ros2_msgs/srv/SpawnUrdfRobot',
+                   message]
 
         super().__init__(
             output=output,

diff --git a/webots_ros2_driver/webots_ros2_driver/webots_launcher.py b/webots_ros2_driver/webots_ros2_driver/webots_launcher.py
@@ -52,14 +52,17 @@ def perform(self, context):
 
 
 class WebotsLauncher(ExecuteProcess):
-    def __init__(self, output='screen', world=None, gui=True, mode='realtime', stream=False, **kwargs):
+    def __init__(self, output='screen', world=None, gui=True, mode='realtime', stream=False, ros2_supervisor=False, **kwargs):
         if sys.platform == 'win32':
             print('WARNING: Native webots_ros2 compatibility with Windows is deprecated and will be removed soon. Please use a '
                   'WSL (Windows Subsystem for Linux) environment instead.', file=sys.stderr)
             print('WARNING: Check https://github.com/cyberbotics/webots_ros2/wiki/Complete-Installation-Guide for more '
                   'information.', file=sys.stderr)
         self.__is_wsl = is_wsl()
         self.__has_shared_folder = has_shared_folder()
+        self.__is_supervisor = ros2_supervisor
+        if self.__is_supervisor:
+            self._supervisor = Ros2SupervisorLauncher()
 
         # Find Webots executable
         if not self.__has_shared_folder:
@@ -89,7 +92,11 @@ def __init__(self, output='screen', world=None, gui=True, mode='realtime', strea
         stdout = _ConditionalSubstitution(condition=gui, false_value='--stdout')
         stderr = _ConditionalSubstitution(condition=gui, false_value='--stderr')
         minimize = _ConditionalSubstitution(condition=gui, false_value='--minimize')
-        stream_argument = _ConditionalSubstitution(condition=stream, true_value='--stream')
+        if isinstance(stream, bool):
+            stream_argument = _ConditionalSubstitution(condition=stream, true_value='--stream')
+        else:
+            stream_argument = "--stream=" + stream
+
         xvfb_run_prefix = []
 
         if 'WEBOTS_OFFSCREEN' in os.environ:
@@ -161,6 +168,10 @@ def execute(self, context: LaunchContext):
                 continue
 
             new_url_path = os.path.split(world_path)[0] + '/' + url_path
+            if self.__has_shared_folder:
+                # Copy asset to shared folder
+                shutil.copy(new_url_path, os.path.join(container_shared_folder(), os.path.basename(new_url_path)))
+                new_url_path = './' + os.path.basename(new_url_path)
             if self.__is_wsl:
                 command = ['wslpath', '-w', new_url_path]
                 new_url_path = subprocess.check_output(command).strip().decode('utf-8').replace('\\', '/')
@@ -172,14 +183,15 @@ def execute(self, context: LaunchContext):
             file.write(content)
 
         # Add the Ros2Supervisor
-        indent = '  '
-        world_file = open(self.__world_copy.name, 'a')
-        world_file.write('Robot {\n')
-        world_file.write(indent + 'name "Ros2Supervisor"\n')
-        world_file.write(indent + 'controller "<extern>"\n')
-        world_file.write(indent + 'supervisor TRUE\n')
-        world_file.write('}\n')
-        world_file.close()
+        if self.__is_supervisor:
+            indent = '  '
+            world_file = open(self.__world_copy.name, 'a')
+            world_file.write('Robot {\n')
+            world_file.write(indent + 'name "Ros2Supervisor"\n')
+            world_file.write(indent + 'controller "<extern>"\n')
+            world_file.write(indent + 'supervisor TRUE\n')
+            world_file.write('}\n')
+            world_file.close()
 
         # Copy world file to shared folder
         if self.__has_shared_folder:

diff --git a/webots_ros2_epuck/CHANGELOG.rst b/webots_ros2_epuck/CHANGELOG.rst
@@ -2,6 +2,10 @@
 Changelog for package webots_ros2_epuck
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2023.0.2 (2023-02-07)
+------------------
+* Updated supervisor launch.
+
 2023.0.1 (2023-01-05)
 ------------------
 * Fixed broken controller connection in Rats life example.

diff --git a/webots_ros2_epuck/launch/robot_launch.py b/webots_ros2_epuck/launch/robot_launch.py
@@ -25,7 +25,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, Ros2SupervisorLauncher
+from webots_ros2_driver.webots_launcher import WebotsLauncher
 from webots_ros2_driver.utils import controller_url_prefix
 
 
@@ -119,16 +119,15 @@ def generate_launch_description():
     world = LaunchConfiguration('world')
 
     webots = WebotsLauncher(
-        world=PathJoinSubstitution([package_dir, 'worlds', world])
+        world=PathJoinSubstitution([package_dir, 'worlds', world]),
+        ros2_supervisor=True
     )
 
-    ros2_supervisor = Ros2SupervisorLauncher()
-
     # The following line is important!
     # This event handler respawns the ROS 2 nodes on simulation reset (supervisor process ends).
     reset_handler = launch.actions.RegisterEventHandler(
         event_handler=launch.event_handlers.OnProcessExit(
-            target_action=ros2_supervisor,
+            target_action=webots._supervisor,
             on_exit=get_ros2_nodes,
         )
     )
@@ -140,7 +139,7 @@ def generate_launch_description():
             description='Choose one of the world files from `/webots_ros2_epuck/world` directory'
         ),
         webots,
-        ros2_supervisor,
+        webots._supervisor,
 
         # This action will kill all nodes once the Webots simulation has exited
         launch.actions.RegisterEventHandler(

diff --git a/webots_ros2_epuck/package.xml b/webots_ros2_epuck/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>webots_ros2_epuck</name>
-  <version>2023.0.1</version>
+  <version>2023.0.2</version>
   <description>E-puck2 driver for Webots simulated robot</description>
 
   <maintainer email="[email protected]">Cyberbotics</maintainer>

diff --git a/webots_ros2_epuck/setup.py b/webots_ros2_epuck/setup.py
@@ -43,7 +43,7 @@
 
 setup(
     name=package_name,
-    version='2023.0.1',
+    version='2023.0.2',
     packages=[package_name],
     data_files=data_files,
     install_requires=['setuptools', 'launch'],

diff --git a/webots_ros2_importer/package.xml b/webots_ros2_importer/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>webots_ros2_importer</name>
-  <version>2023.0.1</version>
+  <version>2023.0.2</version>
   <description>This package allows to convert URDF and XACRO files into Webots PROTO files.</description>
 
   <maintainer email="[email protected]">Cyberbotics</maintainer>

diff --git a/webots_ros2_importer/setup.py b/webots_ros2_importer/setup.py
@@ -10,7 +10,7 @@
 
 setup(
     name=package_name,
-    version='2023.0.1',
+    version='2023.0.2',
     packages=[package_name, package_name + '.urdf2webots.urdf2webots'],
     data_files=data_files,
     install_requires=[

diff --git a/webots_ros2_mavic/CHANGELOG.rst b/webots_ros2_mavic/CHANGELOG.rst
@@ -2,6 +2,10 @@
 Changelog for package webots_ros2_mavic
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2023.0.2 (2023-02-07)
+------------------
+* Updated supervisor launch.
+
 2022.1.3 (2022-11-02)
 ------------------
 * Added macOS support.