From c65198fa0fa4559d43b6b58f7e4ce1c264a22b9b Mon Sep 17 00:00:00 2001 From: Yannick Goumaz <61198661+ygoumaz@users.noreply.github.com> Date: Tue, 7 Feb 2023 08:56:36 +0100 Subject: [PATCH] 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 <44834743+ad-daniel@users.noreply.github.com> * Update ros2_supervisor.py * clear trailing whitespace * Update changelog --------- Co-authored-by: ad-daniel <44834743+ad-daniel@users.noreply.github.com> --- webots_ros2/CHANGELOG.rst | 6 +- webots_ros2_driver/CHANGELOG.rst | 6 +- .../webots_ros2_driver/ros2_supervisor.py | 77 +++++++++++++++++-- .../webots_ros2_driver/urdf_spawner.py | 28 ++----- webots_ros2_epuck/CHANGELOG.rst | 2 +- webots_ros2_mavic/CHANGELOG.rst | 2 +- webots_ros2_tesla/CHANGELOG.rst | 2 +- webots_ros2_tiago/CHANGELOG.rst | 2 +- webots_ros2_turtlebot/CHANGELOG.rst | 2 +- webots_ros2_universal_robot/CHANGELOG.rst | 3 +- .../config/ur5e/visual_parameters.yaml | 28 +++---- ...robotiq-3f-gripper_articulated_macro.xacro | 4 +- ...-3f-gripper_finger_articulated_macro.xacro | 16 ++-- 13 files changed, 113 insertions(+), 65 deletions(-) diff --git a/webots_ros2/CHANGELOG.rst b/webots_ros2/CHANGELOG.rst index 7d1da3b09..44ebdbc02 100644 --- a/webots_ros2/CHANGELOG.rst +++ b/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) ------------------ diff --git a/webots_ros2_driver/CHANGELOG.rst b/webots_ros2_driver/CHANGELOG.rst index 3739916f2..d89960b92 100644 --- a/webots_ros2_driver/CHANGELOG.rst +++ b/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) ------------------ diff --git a/webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py b/webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py index 94be9397c..9c64e7a87 100755 --- 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,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, diff --git a/webots_ros2_driver/webots_ros2_driver/urdf_spawner.py b/webots_ros2_driver/webots_ros2_driver/urdf_spawner.py index 7e42f0dcc..d28b2d81c 100644 --- 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_epuck/CHANGELOG.rst b/webots_ros2_epuck/CHANGELOG.rst index eac9c699b..848faeda7 100644 --- a/webots_ros2_epuck/CHANGELOG.rst +++ b/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. diff --git a/webots_ros2_mavic/CHANGELOG.rst b/webots_ros2_mavic/CHANGELOG.rst index 3a65f75a7..015d8036e 100644 --- a/webots_ros2_mavic/CHANGELOG.rst +++ b/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. diff --git a/webots_ros2_tesla/CHANGELOG.rst b/webots_ros2_tesla/CHANGELOG.rst index 296f3dbf5..0998ae22e 100644 --- a/webots_ros2_tesla/CHANGELOG.rst +++ b/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. diff --git a/webots_ros2_tiago/CHANGELOG.rst b/webots_ros2_tiago/CHANGELOG.rst index 0eeb522cc..b6be619f5 100644 --- a/webots_ros2_tiago/CHANGELOG.rst +++ b/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. diff --git a/webots_ros2_turtlebot/CHANGELOG.rst b/webots_ros2_turtlebot/CHANGELOG.rst index f16a244f4..25fb6a9ab 100644 --- a/webots_ros2_turtlebot/CHANGELOG.rst +++ b/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. diff --git a/webots_ros2_universal_robot/CHANGELOG.rst b/webots_ros2_universal_robot/CHANGELOG.rst index 59ce4d3ed..3210ca8cc 100644 --- a/webots_ros2_universal_robot/CHANGELOG.rst +++ b/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) diff --git a/webots_ros2_universal_robot/resource/Universal_Robots_ROS2_Driver/ur_description/config/ur5e/visual_parameters.yaml b/webots_ros2_universal_robot/resource/Universal_Robots_ROS2_Driver/ur_description/config/ur5e/visual_parameters.yaml index 5649bb441..b64c80faf 100644 --- a/webots_ros2_universal_robot/resource/Universal_Robots_ROS2_Driver/ur_description/config/ur5e/visual_parameters.yaml +++ b/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 diff --git a/webots_ros2_universal_robot/resource/robotiq/robotiq_3f_gripper_visualization/cfg/robotiq-3f-gripper_articulated_macro.xacro b/webots_ros2_universal_robot/resource/robotiq/robotiq_3f_gripper_visualization/cfg/robotiq-3f-gripper_articulated_macro.xacro index 7e76f7c32..1f549aae2 100644 --- a/webots_ros2_universal_robot/resource/robotiq/robotiq_3f_gripper_visualization/cfg/robotiq-3f-gripper_articulated_macro.xacro +++ b/webots_ros2_universal_robot/resource/robotiq/robotiq_3f_gripper_visualization/cfg/robotiq-3f-gripper_articulated_macro.xacro @@ -16,7 +16,7 @@ there are multiple hands then a prefix followed by an "_" is needed. - + @@ -24,7 +24,7 @@ there are multiple hands then a prefix followed by an "_" is needed. - + diff --git a/webots_ros2_universal_robot/resource/robotiq/robotiq_3f_gripper_visualization/cfg/robotiq-3f-gripper_finger_articulated_macro.xacro b/webots_ros2_universal_robot/resource/robotiq/robotiq_3f_gripper_visualization/cfg/robotiq-3f-gripper_finger_articulated_macro.xacro index cdfa651ef..56e28a390 100644 --- a/webots_ros2_universal_robot/resource/robotiq/robotiq_3f_gripper_visualization/cfg/robotiq-3f-gripper_finger_articulated_macro.xacro +++ b/webots_ros2_universal_robot/resource/robotiq/robotiq_3f_gripper_visualization/cfg/robotiq-3f-gripper_finger_articulated_macro.xacro @@ -13,7 +13,7 @@ finger(i.e. finger_1, etc...). - + @@ -22,7 +22,7 @@ finger(i.e. finger_1, etc...). - + @@ -38,14 +38,14 @@ finger(i.e. finger_1, etc...). - + - + @@ -64,14 +64,14 @@ finger(i.e. finger_1, etc...). - + - + @@ -85,14 +85,14 @@ finger(i.e. finger_1, etc...). - + - +