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

webots_ros2_driver/webots/lib/controller/python/controller/field.py

@@ -264,44 +264,44 @@ def count(self) -> int:
 
     @property
     def value(self) -> typing.Union[bool, int, float, str, typing.List[float]]:
-        if self.type == Field.SF_BOOL:
+        if hasattr(self, "type") and self.type == Field.SF_BOOL:
             return wb.wb_supervisor_field_get_sf_bool(self._ref)
-        elif self.type == Field.SF_INT32:
+        elif hasattr(self, "type") and self.type == Field.SF_INT32:
             return wb.wb_supervisor_field_get_sf_int32(self._ref)
-        elif self.type == Field.SF_FLOAT:
+        elif hasattr(self, "type") and self.type == Field.SF_FLOAT:
             return wb.wb_supervisor_field_get_sf_float(self._ref)
-        elif self.type == Field.SF_STRING:
+        elif hasattr(self, "type") and self.type == Field.SF_STRING:
             return wb.wb_supervisor_field_get_sf_string(self._ref).decode()
-        elif self.type == Field.SF_VEC2F:
+        elif hasattr(self, "type") and self.type == Field.SF_VEC2F:
             return wb.wb_supervisor_field_get_sf_vec2f(self._ref)[:2]
-        elif self.type == Field.SF_VEC3F:
+        elif hasattr(self, "type") and self.type == Field.SF_VEC3F:
             return wb.wb_supervisor_field_get_sf_vec3f(self._ref)[:3]
-        elif self.type == Field.SF_ROTATION:
+        elif hasattr(self, "type") and self.type == Field.SF_ROTATION:
             return wb.wb_supervisor_field_get_sf_rotation(self._ref)[:4]
-        elif self.type == Field.SF_COLOR:
+        elif hasattr(self, "type") and self.type == Field.SF_COLOR:
             return wb.wb_supervisor_field_get_sf_color(self._ref)[:3]
-        elif self.type == Field.SF_NODE:
+        elif hasattr(self, "type") and self.type == Field.SF_NODE:
             return self.getSFNode()
         else:
             return None
 
     @value.setter
     def value(self, value: typing.Union[bool, int, float, str, typing.List[float]]):
-        if self.type == Field.SF_BOOL and isinstance(value, bool):
+        if hasattr(self, "type") and self.type == Field.SF_BOOL and isinstance(value, bool):
             wb.wb_supervisor_field_set_sf_bool(self._ref, 1 if value else 0)
-        elif self.type == Field.SF_INT32 and isinstance(value, int):
+        elif hasattr(self, "type") and self.type == Field.SF_INT32 and isinstance(value, int):
             wb.wb_supervisor_field_set_sf_int32(self._ref, value)
-        elif self.type == Field.SF_FLOAT and isinstance(value, float):
+        elif hasattr(self, "type") and self.type == Field.SF_FLOAT and isinstance(value, float):
             wb.wb_supervisor_field_set_sf_float(self._ref, ctypes.c_double(value))
-        elif self.type == Field.SF_STRING and isinstance(value, str):
+        elif hasattr(self, "type") and self.type == Field.SF_STRING and isinstance(value, str):
             wb.wb_supervisor_field_set_sf_string(self._ref, str.encode(value))
-        elif self.type == Field.SF_VEC2F and isinstance(value, list) and len(value) == 2:
+        elif hasattr(self, "type") and self.type == Field.SF_VEC2F and isinstance(value, list) and len(value) == 2:
             wb.wb_supervisor_field_set_sf_vec2f(self._ref, (ctypes.c_double * 2)(*value))
-        elif self.type == Field.SF_VEC3F and isinstance(value, list) and len(value) == 3:
+        elif hasattr(self, "type") and self.type == Field.SF_VEC3F and isinstance(value, list) and len(value) == 3:
             wb.wb_supervisor_field_set_sf_vec3f(self._ref, (ctypes.c_double * 3)(*value))
-        elif self.type == Field.SF_ROTATION and isinstance(value, list) and len(value) == 4:
+        elif hasattr(self, "type") and self.type == Field.SF_ROTATION and isinstance(value, list) and len(value) == 4:
             wb.wb_supervisor_field_set_sf_rotation(self._ref, (ctypes.c_double * 4)(*value))
-        elif self.type == Field.SF_COLOR and isinstance(value, list) and len(value) == 3:
+        elif hasattr(self, "type") and self.type == Field.SF_COLOR and isinstance(value, list) and len(value) == 3:
             wb.wb_supervisor_field_set_sf_color(self._ref, (ctypes.c_double * 3)(*value))
         else:
             print("Error: new field value has wrong type or length.", file=sys.stderr)

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,18 @@ 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 / VRML 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 +75,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 +100,62 @@ 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 VRML 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)
+        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:

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}

webots_ros2_msgs/srv/SpawnNodeFromString.srv

@@ -0,0 +1,3 @@
+string data
+---
+bool success