🐛 trying to fix issue #150

suave/suave/task_bridge.py

@@ -78,10 +78,14 @@ def forward_task_cancel_request(self, function):
         return False
 
     def call_service(self, cli, request):
-        while not cli.wait_for_service(timeout_sec=1.0):
-            self.get_logger().info('service not available, waiting again...')
+        if cli.wait_for_service(timeout_sec=5.0) is False:
+            self.get_logger().error(
+                'service not available {}'.format(cli.srv_name))
+            return None
         future = cli.call_async(request)
-        while self.executor.spin_until_future_complete(
-                future, timeout_sec=1.0):
-            self.get_logger().info("Waiting for future to complete")
+        self.executor.spin_until_future_complete(future, timeout_sec=5.0)
+        if future.done() is False:
+            self.get_logger().error(
+                'Future not completed {}'.format(cli.srv_name))
+            return None
         return future.result()

suave_metacontrol/suave_metacontrol/task_bridge_metacontrol.py

@@ -6,7 +6,7 @@
 from mros2_msgs.action import ControlQos
 
 from rclpy.executors import MultiThreadedExecutor
-from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
+from rclpy.callback_groups import ReentrantCallbackGroup
 from rclpy.action import ActionClient
 from suave.task_bridge import TaskBridge
 
@@ -15,10 +15,10 @@ class TaskBridgeMetacontrol(TaskBridge):
     def __init__(self):
         super().__init__()
 
-        client_cb_group = MutuallyExclusiveCallbackGroup()
+        self.client_cb_group = ReentrantCallbackGroup()
         self.mros_action_client = ActionClient(
             self, ControlQos, '/mros/objective',
-            callback_group=client_cb_group)
+            callback_group=self.client_cb_group)
 
         self.current_objectives_handle = dict()
         self.task_functions_dict = {

suave_missions/config/mission_config.yaml

@@ -10,6 +10,7 @@
 
 /water_visibility_observer_node:
   ros__parameters:
+    qa_publishing_period: 1.0
     water_visibility_period: 80 #Water visibility period in seconds
     water_visibility_min: 1.25 #Minimum value for water visibility
     water_visibility_max: 3.75 #Maximum value for water visibility