diff --git a/webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py b/webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py index 73c562fcd..2cb140105 100755 --- a/webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py +++ b/webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py @@ -32,7 +32,8 @@ from std_msgs.msg import String sys.path.insert(1, os.path.join(os.path.dirname(webots_ros2_importer.__file__), 'urdf2webots')) from urdf2webots.importer import convertUrdfFile, convertUrdfContent -from webots_ros2_msgs.srv import SpawnUrdfRobot +from webots_ros2_msgs.srv import SpawnUrdfRobot, SpawnNodeFromString +import re # As Ros2Supervisor needs the controller library, we extend the path here # to avoid to load another library named "controller" or "vehicle". @@ -52,12 +53,19 @@ def __init__(self): self.create_timer(1 / 1000, self.__supervisor_step_callback) self.__clock_publisher = self.create_publisher(Clock, 'clock', 10) - # Spawn URDF robots + # Spawn Nodes (URDF robots or Webots objects) root_node = self.__robot.getRoot() - self.__insertion_robot_place = root_node.getField('children') - self.__urdf_robots_list=[] + self.__insertion_node_place = root_node.getField('children') + self.__node_list=[] + + + # Services self.create_service(SpawnUrdfRobot, 'spawn_urdf_robot', self.__spawn_urdf_robot_callback) - self.create_subscription(String, 'remove_urdf_robot', self.__remove_urdf_robot_callback, qos_profile_services_default) + 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) + + def __spawn_urdf_robot_callback(self, request, response): robot = request.robot @@ -68,7 +76,7 @@ def __spawn_urdf_robot_callback(self, request, response): self.get_logger().info('Ros2Supervisor cannot import an unnamed URDF robot. Please specifiy it with name="" in the URDFSpawner object.') response.success = False return response - if robot_name in self.__urdf_robots_list: + if robot_name in self.__node_list: self.get_logger().info('The URDF robot name "' + str(robot_name) + '" is already used by another robot! Please specifiy a unique name.') response.success = False return response @@ -93,33 +101,77 @@ def __spawn_urdf_robot_callback(self, request, response): 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 - - self.__insertion_robot_place.importMFNodeFromString(-1, robot_string) + self.__insertion_node_place.importMFNodeFromString(-1, robot_string) self.get_logger().info('Ros2Supervisor has imported the URDF robot named "' + str(robot_name) + '".') - self.__urdf_robots_list.append(robot_name) + self.__node_list.append(robot_name) + response.success = True + return response + + + def __spawn_node_from_string_callback(self, request, response): + object_string = request.data + if(object_string == ''): + self.get_logger().info('Ros2Supervisor cannot import an empty string.') + response.success = False + return response + # Extract Webots node name from string. + name_match = re.search('name "[a-z0-9_]*"', object_string) + object_name = name_match.group().replace('name ', '') + object_name = object_name.replace('"', '') + # Check that the name is not an empty string. + if object_name == '': + self.get_logger().info('Ros2Supervisor cannot import an unnamed node.') + response.success = False + return response + # Check that the name is unique. + if object_name in self.__node_list: + self.get_logger().info('Ros2Supervisor has found a duplicate node in the world named "' + str(object_name) + '". Please specifiy a unique name.') + response.success = False + return response + # Insert the object. + self.__node_list.append(object_name) + self.__insertion_node_place.importMFNodeFromString(-1, object_string) + + # Check if the object has been imported into the world + node = None + node_imported_successfully = False + for id_node in range(self.__insertion_node_place.getCount()): + node = self.__insertion_node_place.getMFNode(id_node) + node_name_field = node.getField('name') + if node_name_field and node_name_field.getSFString() == object_name: + node_imported_successfully = True + break + if not node_imported_successfully: + self.__node_list.remove(object_name) + self.get_logger().info('Ros2Supervisor could not import the node named "' + str(object_name) + '".') + response.success = False + return response + + self.get_logger().info('Ros2Supervisor has imported the node named "' + str(object_name) + '".') response.success = True return response - def __remove_urdf_robot_callback(self, message): - robotName = message.data + # Allows to remove any imported node (urdf robots / VRML Nodes) by name. + def __remove_imported_node_callback(self, message): + name = message.data - if robotName in self.__urdf_robots_list: - robot_node = None + if name in self.__node_list: + node = None - for id_node in range(self.__insertion_robot_place.getCount()): - node = self.__insertion_robot_place.getMFNode(id_node) + for id_node in range(self.__insertion_node_place.getCount()): + node = self.__insertion_node_place.getMFNode(id_node) node_name_field = node.getField('name') - if node_name_field and node_name_field.getSFString() == robotName: - robot_node = node + if node_name_field and node_name_field.getSFString() == name: + node = node break - if robot_node: - robot_node.remove() - self.__urdf_robots_list.remove(robotName) - self.get_logger().info('Ros2Supervisor has removed the URDF robot named "' + str(robotName) + '".') + if node: + node.remove() + self.__node_list.remove(name) + self.get_logger().info('Ros2Supervisor has removed the node named "' + str(name) + '".') else: - self.get_logger().info('Ros2Supervisor wanted to remove the URDF robot named "' + str(robotName) + - '" but this robot has not been found in the simulation world.') + self.get_logger().info('Ros2Supervisor wanted to remove the node named "' + str(name) + + '" but this node has not been found in the simulation world.') def __supervisor_step_callback(self): if self.__robot.step(self.__timestep) < 0: diff --git a/webots_ros2_msgs/CMakeLists.txt b/webots_ros2_msgs/CMakeLists.txt index 6762f94ad..853124bf7 100644 --- a/webots_ros2_msgs/CMakeLists.txt +++ b/webots_ros2_msgs/CMakeLists.txt @@ -30,6 +30,7 @@ set(msg_files set(srv_files "srv/SetInt.srv" "srv/SpawnUrdfRobot.srv" + "srv/SpawnNodeFromString.srv" ) rosidl_generate_interfaces(${PROJECT_NAME} diff --git a/webots_ros2_msgs/srv/SpawnNodeFromString.srv b/webots_ros2_msgs/srv/SpawnNodeFromString.srv new file mode 100644 index 000000000..faf61bb0e --- /dev/null +++ b/webots_ros2_msgs/srv/SpawnNodeFromString.srv @@ -0,0 +1,3 @@ +string data +--- +bool success