🐛 solve issue #150

kas-lab · Feb 19, 2024 · 0288071 · 0288071
1 parent 2fa61c8
commit 0288071
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 11 deletions.
diff --git a/suave/suave/task_bridge.py b/suave/suave/task_bridge.py
@@ -43,7 +43,7 @@ def task_request(self, req, forward_request):
             for function in function_names:
                 forward_request_result = forward_request(function)
                 if type(forward_request_result) is bool:
-                    success = forward_request(function) and success
+                    success = forward_request_result and success
                 else:
                     success = False
             response.success = success

diff --git a/suave_metacontrol/suave_metacontrol/task_bridge_metacontrol.py b/suave_metacontrol/suave_metacontrol/task_bridge_metacontrol.py
@@ -27,14 +27,16 @@ def __init__(self):
         }
 
     def forward_task_request(self, function):
-        self.get_logger().info("Waiting for future to complete")
         future = self.send_mros_objective(function)
 
+        self.get_logger().info("Waiting for mros_objective future to complete")
         self.executor.spin_until_future_complete(future, timeout_sec=5.0)
         if future.done() is False:
             self.get_logger().warning(
                 'Future send mros_objective not completed {}'.format(function))
             return None
+        self.get_logger().info(
+            'Future send mros_objective completed {}'.format(function))
         self.current_objectives_handle[function] = future.result()
         return self.current_objectives_handle[function].accepted
 

diff --git a/suave_missions/suave_missions/mission_planner.py b/suave_missions/suave_missions/mission_planner.py
@@ -1,5 +1,4 @@
-import csv
-from pathlib import Path
+from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
 from rclpy.node import Node
 from suave_msgs.srv import Task
 
@@ -8,10 +7,11 @@ class MissionPlanner(Node):
     def __init__(self, node_name='mission_node'):
         super().__init__(node_name)
 
+        self.cb_group = MutuallyExclusiveCallbackGroup()
         self.task_request_service = self.create_client(
-            Task, 'task/request')
+            Task, 'task/request', callback_group=self.cb_group)
         self.task_cancel_service = self.create_client(
-            Task, 'task/cancel')
+            Task, 'task/cancel', callback_group=self.cb_group)
 
         self.declare_parameter('result_path', '~/suave/results')
         self.declare_parameter('result_filename', 'mission_results')
@@ -35,12 +35,16 @@ def cancel_task(self, task_name):
         return self.call_service(self.task_cancel_service, req)
 
     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()
 
     def perform_mission(self):