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]>

webots_ros2/CHANGELOG.rst

@@ -2,14 +2,12 @@
 Changelog for package webots_ros2
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-2023.0.3 (2023-XX-XX)
-------------------
-* Ros2Supervisor is now optional.
-
 2023.0.2 (2023-XX-XX)
 ------------------
 * 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)
 ------------------

webots_ros2_driver/CHANGELOG.rst

@@ -2,13 +2,11 @@
 Changelog for package webots_ros2_driver
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-2023.0.3 (2023-XX-XX)
-------------------
-* Added Ros2Supervisor creation.
-
 2023.0.2 (2023-XX-XX)
 ------------------
+* 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)
 ------------------

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,77 @@ 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,

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,

webots_ros2_epuck/CHANGELOG.rst

@@ -2,7 +2,7 @@
 Changelog for package webots_ros2_epuck
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-2023.0.3 (2023-XX-XX)
+2023.0.2 (2023-XX-XX)
 ------------------
 * Updated supervisor launch.
 

webots_ros2_mavic/CHANGELOG.rst

@@ -2,7 +2,7 @@
 Changelog for package webots_ros2_mavic
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-2023.0.3 (2023-XX-XX)
+2023.0.2 (2023-XX-XX)
 ------------------
 * Updated supervisor launch.
 

webots_ros2_tesla/CHANGELOG.rst

@@ -2,7 +2,7 @@
 Changelog for package webots_ros2_tesla
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-2023.0.3 (2023-XX-XX)
+2023.0.2 (2023-XX-XX)
 ------------------
 * Updated supervisor launch.
 

webots_ros2_tiago/CHANGELOG.rst

@@ -2,7 +2,7 @@
 Changelog for package webots_ros2_tiago
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-2023.0.3 (2023-XX-XX)
+2023.0.2 (2023-XX-XX)
 ------------------
 * Updated supervisor launch.
 

webots_ros2_turtlebot/CHANGELOG.rst

@@ -2,7 +2,7 @@
 Changelog for package webots_ros2_turtlebot
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-2023.0.3 (2023-XX-XX)
+2023.0.2 (2023-XX-XX)
 ------------------
 * Updated supervisor launch.
 

webots_ros2_universal_robot/CHANGELOG.rst

@@ -2,8 +2,9 @@
 Changelog for package webots_ros2_universal_robot
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-2023.0.3 (2023-XX-XX)
+2023.0.2 (2023-XX-XX)
 ------------------
+* Fixed URDF relative URLs to assets.
 * Updated supervisor launch.
 
 2022.1.3 (2022-11-02)

webots_ros2_universal_robot/resource/Universal_Robots_ROS2_Driver/ur_description/config/ur5e/visual_parameters.yaml

@@ -1,67 +1,67 @@
 mesh_files:
   base:
     visual:
-      mesh: "package://resource/Universal_Robots_ROS2_Driver/ur_description/meshes/ur5e/visual/base.dae"
+      mesh: "package://webots_ros2_universal_robot/resource/Universal_Robots_ROS2_Driver/ur_description/meshes/ur5e/visual/base.dae"
       material:
         name: "LightGrey"
         color: "0.7 0.7 0.7 1.0"
     collision:
-      mesh: "package://resource/Universal_Robots_ROS2_Driver/ur_description/meshes/ur5e/collision/base.stl"
+      mesh: "package://webots_ros2_universal_robot/resource/Universal_Robots_ROS2_Driver/ur_description/meshes/ur5e/collision/base.stl"
 
   shoulder:
     visual:
-      mesh: "package://resource/Universal_Robots_ROS2_Driver/ur_description/meshes/ur5e/visual/shoulder.dae"
+      mesh: "package://webots_ros2_universal_robot/resource/Universal_Robots_ROS2_Driver/ur_description/meshes/ur5e/visual/shoulder.dae"
       material:
         name: "LightGrey"
         color: "0.7 0.7 0.7 1.0"
     collision:
-      mesh: "package://resource/Universal_Robots_ROS2_Driver/ur_description/meshes/ur5e/collision/shoulder.stl"
+      mesh: "package://webots_ros2_universal_robot/resource/Universal_Robots_ROS2_Driver/ur_description/meshes/ur5e/collision/shoulder.stl"
 
   upper_arm:
     visual:
-      mesh: "package://resource/Universal_Robots_ROS2_Driver/ur_description/meshes/ur5e/visual/upperarm.dae"
+      mesh: "package://webots_ros2_universal_robot/resource/Universal_Robots_ROS2_Driver/ur_description/meshes/ur5e/visual/upperarm.dae"
       material:
         name: "LightGrey"
         color: "0.7 0.7 0.7 1.0"
     collision:
-      mesh: "package://resource/Universal_Robots_ROS2_Driver/ur_description/meshes/ur5e/collision/upperarm.stl"
+      mesh: "package://webots_ros2_universal_robot/resource/Universal_Robots_ROS2_Driver/ur_description/meshes/ur5e/collision/upperarm.stl"
       mesh_files:
 
   forearm:
     visual:
-      mesh: "package://resource/Universal_Robots_ROS2_Driver/ur_description/meshes/ur5e/visual/forearm.dae"
+      mesh: "package://webots_ros2_universal_robot/resource/Universal_Robots_ROS2_Driver/ur_description/meshes/ur5e/visual/forearm.dae"
       material:
         name: "LightGrey"
         color: "0.7 0.7 0.7 1.0"
     collision:
-      mesh: "package://resource/Universal_Robots_ROS2_Driver/ur_description/meshes/ur5e/collision/forearm.stl"
+      mesh: "package://webots_ros2_universal_robot/resource/Universal_Robots_ROS2_Driver/ur_description/meshes/ur5e/collision/forearm.stl"
 
   wrist_1:
     visual:
-      mesh: "package://resource/Universal_Robots_ROS2_Driver/ur_description/meshes/ur5e/visual/wrist1.dae"
+      mesh: "package://webots_ros2_universal_robot/resource/Universal_Robots_ROS2_Driver/ur_description/meshes/ur5e/visual/wrist1.dae"
       material:
         name: "LightGrey"
         color: "0.7 0.7 0.7 1.0"
     collision:
-      mesh: "package://resource/Universal_Robots_ROS2_Driver/ur_description/meshes/ur5e/collision/wrist1.stl"
+      mesh: "package://webots_ros2_universal_robot/resource/Universal_Robots_ROS2_Driver/ur_description/meshes/ur5e/collision/wrist1.stl"
     visual_offset: -0.127
 
   wrist_2:
     visual:
-      mesh: "package://resource/Universal_Robots_ROS2_Driver/ur_description/meshes/ur5e/visual/wrist2.dae"
+      mesh: "package://webots_ros2_universal_robot/resource/Universal_Robots_ROS2_Driver/ur_description/meshes/ur5e/visual/wrist2.dae"
       material:
         name: "LightGrey"
         color: "0.7 0.7 0.7 1.0"
     collision:
-      mesh: "package://resource/Universal_Robots_ROS2_Driver/ur_description/meshes/ur5e/collision/wrist2.stl"
+      mesh: "package://webots_ros2_universal_robot/resource/Universal_Robots_ROS2_Driver/ur_description/meshes/ur5e/collision/wrist2.stl"
     visual_offset: -0.0997
 
   wrist_3:
     visual:
-      mesh: "package://resource/Universal_Robots_ROS2_Driver/ur_description/meshes/ur5e/visual/wrist3.dae"
+      mesh: "package://webots_ros2_universal_robot/resource/Universal_Robots_ROS2_Driver/ur_description/meshes/ur5e/visual/wrist3.dae"
       material:
         name: "LightGrey"
         color: "0.7 0.7 0.7 1.0"
     collision:
-      mesh: "package://resource/Universal_Robots_ROS2_Driver/ur_description/meshes/ur5e/collision/wrist3.stl"
+      mesh: "package://webots_ros2_universal_robot/resource/Universal_Robots_ROS2_Driver/ur_description/meshes/ur5e/collision/wrist3.stl"
     visual_offset: -0.0989

webots_ros2_universal_robot/resource/robotiq/robotiq_3f_gripper_visualization/cfg/robotiq-3f-gripper_articulated_macro.xacro

@@ -16,15 +16,15 @@ there are multiple hands then a prefix followed by an "_" is needed.
 		<link name="${prefix}palm">
 			<visual>
 				<geometry>
-                                        <mesh filename="package://resource/robotiq/robotiq_3f_gripper_visualization/meshes/robotiq-3f-gripper_articulated/visual/palm.dae" />
+                                        <mesh filename="package://webots_ros2_universal_robot/resource/robotiq/robotiq_3f_gripper_visualization/meshes/robotiq-3f-gripper_articulated/visual/palm.dae" />
 				</geometry>
 				<material name="gray">
 					<color rgba="0.2 0.2 0.2 1"/>
 				</material>
 			</visual>
 			<collision>
 				<geometry>
-                                        <mesh filename="package://resource/robotiq/robotiq_3f_gripper_visualization/meshes/robotiq-3f-gripper_articulated/collision/palm.STL" />
+                                        <mesh filename="package://webots_ros2_universal_robot/resource/robotiq/robotiq_3f_gripper_visualization/meshes/robotiq-3f-gripper_articulated/collision/palm.STL" />
 				</geometry>
 				<material name="yellow">
 					<color rgba="0 1 1 1"/>