From 78cfb01a4b3127565d28dae67177d424c9919698 Mon Sep 17 00:00:00 2001 From: Matthias Mayr Date: Tue, 24 Sep 2024 20:29:50 +0000 Subject: [PATCH] New: WMI gets init scene name WM server in ROS 2 We can not just read that node parameter like in ROS 1. Created a server to ask for the init_scene's name. --- skiros2_msgs/CMakeLists.txt | 1 + skiros2_msgs/srv/WmInitScene.srv | 2 ++ .../skiros2_world_model/ros/world_model_interface.py | 6 ++++-- .../skiros2_world_model/ros/world_model_server.py | 11 ++++++++--- 4 files changed, 15 insertions(+), 5 deletions(-) create mode 100644 skiros2_msgs/srv/WmInitScene.srv diff --git a/skiros2_msgs/CMakeLists.txt b/skiros2_msgs/CMakeLists.txt index 6f9eefb0..5445c824 100644 --- a/skiros2_msgs/CMakeLists.txt +++ b/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 diff --git a/skiros2_msgs/srv/WmInitScene.srv b/skiros2_msgs/srv/WmInitScene.srv new file mode 100644 index 00000000..f3a6e0c9 --- /dev/null +++ b/skiros2_msgs/srv/WmInitScene.srv @@ -0,0 +1,2 @@ +--- +string init_scene \ No newline at end of file diff --git a/skiros2_world_model/skiros2_world_model/ros/world_model_interface.py b/skiros2_world_model/skiros2_world_model/ros/world_model_interface.py index bb9ad0e0..1a61d64f 100644 --- a/skiros2_world_model/skiros2_world_model/ros/world_model_interface.py +++ b/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): """ diff --git a/skiros2_world_model/skiros2_world_model/ros/world_model_server.py b/skiros2_world_model/skiros2_world_model/ros/world_model_server.py index 3173df9e..53b9e376 100644 --- a/skiros2_world_model/skiros2_world_model/ros/world_model_server.py +++ b/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)