🐛 fix task bridge srv cbs

kas-lab · Jan 17, 2024 · 9d8becb · 9d8becb
1 parent d17eac9
commit 9d8becb
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 11 deletions.
diff --git a/suave/suave/task_bridge.py b/suave/suave/task_bridge.py
@@ -27,6 +27,16 @@ def __init__(self):
 
     def task_request(self, req, forward_request):
         response = Task.Response()
+        if req.task_name not in self.task_functions_dict:
+            self.get_logger().error(
+                'Request: {}'.format(req) +
+                'Task requested is not available. ' +
+                'Available tasks: {}'.format(self.task_functions_dict.keys())
+            )
+            response = Task.Response()
+            response.success = False
+            return response
+
         try:
             function_names = self.task_functions_dict[req.task_name]
             success = True
@@ -35,9 +45,7 @@ def task_request(self, req, forward_request):
             response.success = success
         except Exception as e:
             self.get_logger().error(
-                'Exception: {}. '.format(e) +
-                'Probably requested wrong task name. ' +
-                'Available tasks: {}'.format(self.task_functions_dict.keys())
+                'Exception: {0}. Request: {1}'.format(e, req)
             )
             response = Task.Response()
             response.success = False

diff --git a/suave_metacontrol/suave_metacontrol/task_bridge_metacontrol.py b/suave_metacontrol/suave_metacontrol/task_bridge_metacontrol.py
@@ -29,9 +29,12 @@ def __init__(self):
     def forward_task_request(self, function):
         self.get_logger().info("Waiting for future to complete")
         future = self.send_mros_objective(function)
-        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().warning(
+                'Future send mros_objective not completed {}'.format(function))
+            return None
         self.current_objectives_handle[function] = future.result()
         return self.current_objectives_handle[function].accepted
 
@@ -41,14 +44,16 @@ def forward_task_cancel_request(self, function):
             goal_handle = self.current_objectives_handle[function]
             future = goal_handle.cancel_goal_async()
             self.get_logger().info('cancel requested {}'.format(function))
-            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().warning(
+                    'Future cancel mros_objective not completed {}'.format(
+                        function))
+                return False
             del function
             if future is not None and goal_handle.status == 3:
                 return True
-            else:
-                return False
+            return False
         else:
             self.get_logger().info(
                 'Function {} is not active'.format(function))