Merge pull request #144 from kas-lab/issue_142

Issue 142

suave.rosinstall

@@ -30,7 +30,7 @@ repositories:
   mc_mros_reasoner:
     type: git
     url: https://github.com/meta-control/mc_mros_reasoner.git
-    version: 9392f680fb33ed97f6105830dbb5593e68070419
+    version: 9b1f915
   system_modes:
     type: git
     url: https://github.com/micro-ROS/system_modes

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

suave_metacontrol/launch/metacontrol.launch.py

@@ -6,8 +6,6 @@
 from launch.actions import IncludeLaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 
-from launch_ros.actions import Node
-
 
 def generate_launch_description():
 

suave_metacontrol/suave_metacontrol/suave_reasoner.py

@@ -35,7 +35,7 @@ def analyze(self):
                                 objective.o_status = "IN_ERROR_NFR"
         except Exception as err:
             self.logger.info(
-                "In Custom Analyze, exception returned: {}".format(err))
+                "In custom Analyze, exception returned: {}".format(err))
         return objectives_in_error
 
 
@@ -54,7 +54,7 @@ def main(args=None):
 
     # Spin until the process in terminated
     rclpy.spin(pipeline_inspection_reasoner, executor=mt_executor)
-    ros_reasoner.destroy_node()
+    pipeline_inspection_reasoner.destroy_node()
     rclpy.shutdown()
 
 

suave_metacontrol/suave_metacontrol/task_bridge_metacontrol.py

@@ -2,15 +2,12 @@
 import sys
 import rclpy
 
-from diagnostic_msgs.msg import DiagnosticArray
-from diagnostic_msgs.msg import DiagnosticStatus
 from diagnostic_msgs.msg import KeyValue
 from mros2_msgs.action import ControlQos
 
 from rclpy.executors import MultiThreadedExecutor
 from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
 from rclpy.action import ActionClient
-from rclpy.node import Node
 from suave.task_bridge import TaskBridge
 
 
@@ -32,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
 
@@ -44,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))