From 3316357a6d1461c7e4dbfc95ccc3abb74877f17f Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Fri, 7 Jul 2023 10:26:21 +0200 Subject: [PATCH 1/8] add new proto spawner --- .../webots_ros2_driver/proto_spawner.py | 53 ++++++++++++ .../webots_ros2_driver/ros2_supervisor.py | 85 ++++++++++++++++++- webots_ros2_msgs/msg/ProtoRobot.msg | 2 + webots_ros2_msgs/srv/SpawnProtoRobot.srv | 3 + 4 files changed, 141 insertions(+), 2 deletions(-) create mode 100644 webots_ros2_driver/webots_ros2_driver/proto_spawner.py create mode 100644 webots_ros2_msgs/msg/ProtoRobot.msg create mode 100644 webots_ros2_msgs/srv/SpawnProtoRobot.srv diff --git a/webots_ros2_driver/webots_ros2_driver/proto_spawner.py b/webots_ros2_driver/webots_ros2_driver/proto_spawner.py new file mode 100644 index 000000000..0df0d1660 --- /dev/null +++ b/webots_ros2_driver/webots_ros2_driver/proto_spawner.py @@ -0,0 +1,53 @@ +#!/usr/bin/env python + +# Copyright 1996-2023 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""This process simply sends urdf information to the Spawner through a service.""" + +from launch.actions import ExecuteProcess + + +def get_webots_driver_node(event, driver_node): + """Return the driver node in case the service response is successful.""" + if 'success=True' in event.text.decode().strip(): + return driver_node + if 'success=False' in event.text.decode().strip(): + print('WARNING: the Ros2Supervisor was not able to spawn this URDF robot.') + return + + +class PROTOSpawner(ExecuteProcess): + def __init__(self, output='log', name=None, proto_path=None, robot_string=None, **kwargs): + message = '{robot: {' + + if proto_path: + message += 'proto_path: "' + proto_path + '",' + elif robot_string: + message += 'robot_string: "\\\n' + robot_string + '",' + + message += '} }' + + command = ['ros2', + 'service', + 'call', + '/Ros2Supervisor/spawn_urdf_robot', + 'webots_ros2_msgs/srv/SpawnProtoRobot', + message] + + super().__init__( + output=output, + cmd=command, + **kwargs + ) diff --git a/webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py b/webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py index 9c64e7a87..f2ce14d08 100644 --- a/webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py +++ b/webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py @@ -64,7 +64,7 @@ def __init__(self): # Services self.create_service(SpawnUrdfRobot, 'spawn_urdf_robot', self.__spawn_urdf_robot_callback) - self.create_service(SpawnNodeFromString, 'spawn_node_from_string', self.__spawn_node_from_string_callback) + self.create_service(SpawnNodeFromString, 'spawn_proto_robot', self.__spawn_proto_robot_callback) # Subscriptions self.create_subscription(String, 'remove_node', self.__remove_imported_node_callback, qos_profile_services_default) @@ -176,7 +176,88 @@ def __spawn_urdf_robot_callback(self, request, response): response.success = True return response - def __spawn_node_from_string_callback(self, request, response): + def __spawn_proto_robot_callback(self, request, response): + robot = request.robot + # Choose the conversion according to the input and platform + if robot.proto_path: + if has_shared_folder() or is_wsl(): + # Check that the file exists and is an URDF + if not os.path.isfile(robot.proto_path): + sys.exit('Input file "%s" does not exist.' % robot.proto_path) + if not robot.proto_path.endswith('.urdf'): + sys.exit('"%s" is not a URDF file.' % robot.proto_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, + relativePathPrefix=relative_path_prefix) + else: + self.get_logger().info('Ros2Supervisor can not import a URDF file without a specified "urdf_path" or ' + '"robot_description" in the URDFSpawner object.') + response.success = False + return response object_string = request.data if object_string == '': self.get_logger().info('Ros2Supervisor cannot import an empty string.') diff --git a/webots_ros2_msgs/msg/ProtoRobot.msg b/webots_ros2_msgs/msg/ProtoRobot.msg new file mode 100644 index 000000000..912fd8c13 --- /dev/null +++ b/webots_ros2_msgs/msg/ProtoRobot.msg @@ -0,0 +1,2 @@ +string proto_path +string robot_string diff --git a/webots_ros2_msgs/srv/SpawnProtoRobot.srv b/webots_ros2_msgs/srv/SpawnProtoRobot.srv new file mode 100644 index 000000000..1657b2db7 --- /dev/null +++ b/webots_ros2_msgs/srv/SpawnProtoRobot.srv @@ -0,0 +1,3 @@ +UrdfRobot robot +--- +bool success From 1e41bf02eb43141b18d0699c1da34c4142268805 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz <61198661+ygoumaz@users.noreply.github.com> Date: Fri, 21 Jul 2023 14:43:06 +0200 Subject: [PATCH 2/8] Update proto_spawner.py --- webots_ros2_driver/webots_ros2_driver/proto_spawner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/webots_ros2_driver/webots_ros2_driver/proto_spawner.py b/webots_ros2_driver/webots_ros2_driver/proto_spawner.py index 0df0d1660..06428dd00 100644 --- a/webots_ros2_driver/webots_ros2_driver/proto_spawner.py +++ b/webots_ros2_driver/webots_ros2_driver/proto_spawner.py @@ -42,7 +42,7 @@ def __init__(self, output='log', name=None, proto_path=None, robot_string=None, command = ['ros2', 'service', 'call', - '/Ros2Supervisor/spawn_urdf_robot', + '/Ros2Supervisor/spawn_proto_robot', 'webots_ros2_msgs/srv/SpawnProtoRobot', message] From 474e3dc7e791c4727f548e9fbf1e033d22290ef4 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz <61198661+ygoumaz@users.noreply.github.com> Date: Fri, 21 Jul 2023 14:44:18 +0200 Subject: [PATCH 3/8] Update webots_ros2_msgs/srv/SpawnProtoRobot.srv Co-authored-by: Ignacio Davila <99193391+IDavGal@users.noreply.github.com> --- webots_ros2_msgs/srv/SpawnProtoRobot.srv | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/webots_ros2_msgs/srv/SpawnProtoRobot.srv b/webots_ros2_msgs/srv/SpawnProtoRobot.srv index 1657b2db7..6957a290f 100644 --- a/webots_ros2_msgs/srv/SpawnProtoRobot.srv +++ b/webots_ros2_msgs/srv/SpawnProtoRobot.srv @@ -1,3 +1,3 @@ -UrdfRobot robot +ProtoRobot robot --- bool success From 9a3adb562d1cb41032932b2a1a23b6589b137c02 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz <61198661+ygoumaz@users.noreply.github.com> Date: Fri, 21 Jul 2023 14:59:52 +0200 Subject: [PATCH 4/8] Update ProtoRobot.msg --- webots_ros2_msgs/msg/ProtoRobot.msg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/webots_ros2_msgs/msg/ProtoRobot.msg b/webots_ros2_msgs/msg/ProtoRobot.msg index 912fd8c13..a7fc2f74a 100644 --- a/webots_ros2_msgs/msg/ProtoRobot.msg +++ b/webots_ros2_msgs/msg/ProtoRobot.msg @@ -1,2 +1,2 @@ -string proto_path +string proto_paths string robot_string From 21eef442c4f02c8b2d1b34b21d45fa33e28b96fe Mon Sep 17 00:00:00 2001 From: Yannick Goumaz <61198661+ygoumaz@users.noreply.github.com> Date: Fri, 21 Jul 2023 15:10:33 +0200 Subject: [PATCH 5/8] Update ProtoRobot.msg --- webots_ros2_msgs/msg/ProtoRobot.msg | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/webots_ros2_msgs/msg/ProtoRobot.msg b/webots_ros2_msgs/msg/ProtoRobot.msg index a7fc2f74a..35e0bba37 100644 --- a/webots_ros2_msgs/msg/ProtoRobot.msg +++ b/webots_ros2_msgs/msg/ProtoRobot.msg @@ -1,2 +1,2 @@ -string proto_paths -string robot_string +string proto_urls +string node_string From 9afe0ab3fccab4656b7e24b81b86f070f7a15d68 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz <61198661+ygoumaz@users.noreply.github.com> Date: Fri, 21 Jul 2023 15:16:24 +0200 Subject: [PATCH 6/8] Update ros2_supervisor.py --- .../webots_ros2_driver/ros2_supervisor.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py b/webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py index f2ce14d08..20860f0f7 100644 --- a/webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py +++ b/webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py @@ -179,16 +179,17 @@ def __spawn_urdf_robot_callback(self, request, response): def __spawn_proto_robot_callback(self, request, response): robot = request.robot # Choose the conversion according to the input and platform - if robot.proto_path: + if robot.proto_urls: + # TODO: iterate over all urls if has_shared_folder() or is_wsl(): # Check that the file exists and is an URDF - if not os.path.isfile(robot.proto_path): - sys.exit('Input file "%s" does not exist.' % robot.proto_path) + if not os.path.isfile(robot.proto_urls): + sys.exit('Input file "%s" does not exist.' % robot.proto_urls) if not robot.proto_path.endswith('.urdf'): - sys.exit('"%s" is not a URDF file.' % robot.proto_path) + sys.exit('"%s" is not a URDF file.' % robot.proto_urls) # Read the content of the URDF - with open(robot.urdf_path, 'r') as file: + with open(robot.proto_urls, 'r') as file: urdfContent = file.read() if urdfContent is None: sys.exit('Could not read the URDF file.') From e0b2a30719765f71a9dbd69bd5b70b47930bb22b Mon Sep 17 00:00:00 2001 From: Yannick Goumaz <61198661+ygoumaz@users.noreply.github.com> Date: Fri, 21 Jul 2023 15:20:32 +0200 Subject: [PATCH 7/8] Update ros2_supervisor.py --- webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py b/webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py index 20860f0f7..5a6113cf1 100644 --- a/webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py +++ b/webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py @@ -259,7 +259,7 @@ def __spawn_proto_robot_callback(self, request, response): '"robot_description" in the URDFSpawner object.') response.success = False return response - object_string = request.data + object_string = robot.node_string if object_string == '': self.get_logger().info('Ros2Supervisor cannot import an empty string.') response.success = False From 88d65ff471377aa0ab8ae5ed70388ca6e21bc627 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz <61198661+ygoumaz@users.noreply.github.com> Date: Fri, 21 Jul 2023 15:45:00 +0200 Subject: [PATCH 8/8] Update ros2_supervisor.py --- webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py b/webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py index 5a6113cf1..6fb500a6c 100644 --- a/webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py +++ b/webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py @@ -64,7 +64,7 @@ def __init__(self): # Services self.create_service(SpawnUrdfRobot, 'spawn_urdf_robot', self.__spawn_urdf_robot_callback) - self.create_service(SpawnNodeFromString, 'spawn_proto_robot', self.__spawn_proto_robot_callback) + self.create_service(SpawnNodeFromString, 'spawn_node_from_string', self.__spawn_node_from_string_callback) # Subscriptions self.create_subscription(String, 'remove_node', self.__remove_imported_node_callback, qos_profile_services_default) @@ -176,7 +176,7 @@ def __spawn_urdf_robot_callback(self, request, response): response.success = True return response - def __spawn_proto_robot_callback(self, request, response): + def __spawn_node_from_string_callback(self, request, response): robot = request.robot # Choose the conversion according to the input and platform if robot.proto_urls: