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

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, SpawnWbtObject
+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 / WBT Objects)
         root_node = self.__robot.getRoot()
         self.__insertion_robot_place = root_node.getField('children')
-        self.__urdf_robots_list=[]
+        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(SpawnWbtObject, 'spawn_wbt_obj', self.__spawn_wbt_obj_callback)
+
+        # Subscriptions        
+        self.create_subscription(String, 'remove_node', self.__remove_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,61 @@ 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.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_wbt_obj_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 name from wbt model 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 wbt object.')
+            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 wbt object 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_robot_place.importMFNodeFromString(-1, object_string)
+        self.get_logger().info('Ros2Supervisor has imported the wbt object named "' + str(object_name) + '".')
+
         response.success = True
         return response
 
-    def __remove_urdf_robot_callback(self, message):
-        robotName = message.data
+    def __remove_node_callback(self, message):
+        name = message.data
 
-        if robotName in self.__urdf_robots_list:
+        if name in self.__node_list:
             robot_node = None
 
             for id_node in range(self.__insertion_robot_place.getCount()):
                 node = self.__insertion_robot_place.getMFNode(id_node)
                 node_name_field = node.getField('name')
-                if node_name_field and node_name_field.getSFString() == robotName:
+                if node_name_field and node_name_field.getSFString() == name:
                     robot_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) + '".')
+                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/SpawnWbtObject.srv"
 )
 
 rosidl_generate_interfaces(${PROJECT_NAME}

webots_ros2_msgs/srv/SpawnWbtObject.srv

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