New: WMI gets init scene name WM server in ROS 2

skiros2_msgs/CMakeLists.txt

@@ -21,6 +21,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   srv/WoQuery.srv
   srv/WoModify.srv
   srv/WmGet.srv
+  srv/WmInitScene.srv
   srv/WoLoadAndSave.srv
   srv/WmSetRelation.srv
   srv/WmQueryRelations.srv

skiros2_msgs/srv/WmInitScene.srv

@@ -0,0 +1,2 @@
+---
+string init_scene

skiros2_world_model/skiros2_world_model/ros/world_model_interface.py

@@ -33,6 +33,7 @@ def __init__(self, node, author_name, make_cache=False, allow_spinning=True):
         self._get = self._node.create_client(srvs.WmGet, 'wm/get')
         self._modify = self._node.create_client(srvs.WmModify, 'wm/modify')
         self._query_relations = self._node.create_client(srvs.WmQueryRelations, 'wm/scene/query_relations')
+        self._init_scene = self._node.create_client(srvs.WmInitScene, 'wm/init_scene')
         for s in [self._get, self._set_relations, self._modify, self._query_relations]:
             while not s.wait_for_service(timeout_sec=1.0):
                 log.warn("[{}]".format(self.__class__.__name__), "Service {} not available, waiting again ...".format(s.srv_name))
@@ -93,9 +94,10 @@ def _resolve_local_relations(self, e):
 
     def get_scene_name(self):
         """
-        @brief Get the name of the scene from ROS parameter
+        @brief Get the name of the scene from the world-model server
         """
-        return self._node.get_parameter('wm/init_scene').value
+        res = self._call(self._init_scene, srvs.WmInitScene.Request())
+        return res.init_scene
 
     def get_scene(self):
         """

skiros2_world_model/skiros2_world_model/ros/world_model_server.py

@@ -38,6 +38,7 @@ def __init__(self, name='wm'):
         self._query_relations = self.create_service(srvs.WmQueryRelations, '~/scene/query_relations', self._wm_query_rel_cb)
         self._get = self.create_service(srvs.WmGet, '~/get', self._wm_get_cb)
         self._modify = self.create_service(srvs.WmModify, '~/modify', self._wm_modify_cb)
+        self._scene_name = self.create_service(srvs.WmInitScene, '~/init_scene', self._wm_init_scene_cb)
         # TODO: missing latching in ROS2, need to config QoS https://github.com/RobotWebTools/ros2-web-bridge/issues/134
         self._monitor = self.create_publisher(msgs.WmMonitor, "~/monitor", 20)
         self._load_and_save = self.create_service(srvs.WoLoadAndSave, '~/load_and_save', self._load_and_save_cb)
@@ -59,10 +60,10 @@ def _init_wm(self):
         self._ontology.workspace = self._workspace
         log.info("[{}]".format(self.__class__.__name__), "Workspace folder: {}".format(self._workspace))
         self._ontology.set_default_prefix('skiros', 'http://rvmi.aau.dk/ontologies/skiros.owl#')
-        init_scene = self.get_parameter('init_scene').value
+        self._init_scene = self.get_parameter('init_scene').value
         self._ontology.reset()
-        if init_scene != "":
-            self._ontology.load_context(init_scene)
+        if self._init_scene != "":
+            self._ontology.load_context(self._init_scene)
         for context in self.get_parameter('load_contexts').value:
             if context:  # TODO: understand why it is returning an empty string?
                 context_id, filename = context.split(" ")
@@ -211,6 +212,10 @@ def _wm_modify_cb(self, msg, to_ret):
         if self._verbose:
             log.info("[WmModify]", "{} {} {}. Time: {:0.3f} secs".format(msg.author, msg.action, [e.id for e in to_ret.elements], self._times.get_last()))
         return to_ret
+
+    def _wm_init_scene_cb(self, _, to_ret):
+        to_ret.init_scene = self._init_scene        
+        return to_ret
 
     def run(self):
         rclpy.spin(self)