Added ability to import objects with webots wbt type. Refactored the … (

#539) * Added ability to import objects with webots mdt type. Refactored the superviser file to allow for this feature addition. Signed-off-by: TBreeze98 * fixed typo * corrected file location * changed all references of wdt and mdt to wbt * made changes suggested by ygoumaz * added field type check which fixes crash * changed variable names as suggested by ygoumaz * removed modifications of field.py * added check to validate that the object has been imported into the world * Update webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py Co-authored-by: Olivier Michel <[email protected]> * Update webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py Co-authored-by: Olivier Michel <[email protected]> * Update webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py Co-authored-by: Olivier Michel <[email protected]> * Update webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py Co-authored-by: Olivier Michel <[email protected]> * Update webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py Co-authored-by: Olivier Michel <[email protected]> * Update webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py Co-authored-by: Olivier Michel <[email protected]> * Update webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py Co-authored-by: Olivier Michel <[email protected]> * Update webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py Co-authored-by: Olivier Michel <[email protected]> Co-authored-by: Olivier Michel <[email protected]> Co-authored-by: Yannick Goumaz <[email protected]>
cyberbotics · Jan 10, 2023 · 7c978b8 · 7c978b8
1 parent 7b6bdc0
commit 7c978b8
Show file tree

Hide file tree

Showing 3 changed files with 79 additions and 23 deletions.
diff --git 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
@@ -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
@@ -0,0 +1,3 @@
+string data
+---
+bool success