From 237fc86d669c582625725a90f151954e5fffe6eb Mon Sep 17 00:00:00 2001
From: Torstein Lundervold Nesheim <TLNE@equinor.com>
Date: Thu, 9 Jun 2022 13:52:56 +0200
Subject: [PATCH] Add info on robot capabilities

---
 .../apis/schedule/scheduling_controller.py    | 11 +++++++---
 src/isar/mission_planner/mission_validator.py | 20 ++++++++++---------
 .../mission_planner/test_mission_validator.py |  2 +-
 3 files changed, 20 insertions(+), 13 deletions(-)

diff --git a/src/isar/apis/schedule/scheduling_controller.py b/src/isar/apis/schedule/scheduling_controller.py
index ed7a0edd..580a1590 100644
--- a/src/isar/apis/schedule/scheduling_controller.py
+++ b/src/isar/apis/schedule/scheduling_controller.py
@@ -1,7 +1,7 @@
 import logging
 from http import HTTPStatus
 from queue import Empty
-from typing import List, Optional
+from typing import List, Optional, Tuple
 
 import numpy as np
 from alitra import Frame, Orientation, Pose, Position
@@ -90,11 +90,16 @@ def start_mission(
             response.status_code = HTTPStatus.INTERNAL_SERVER_ERROR.value
             return
 
-        robot_capable: bool = is_robot_capable_of_mission(
+        robot_capable: bool
+        missing_functions: List[str]
+        (robot_capable, missing_functions) = is_robot_capable_of_mission(
             mission=mission, robot_capabilities=robot_settings.CAPABILITIES
         )
         if not robot_capable:
-            errorMsg = "Bad Request - Robot is not capable of performing mission"
+            errorMsg = (
+                "Bad Request - Robot is not capable of performing mission. Missing functionalities: "
+                + str(missing_functions)
+            )
             self.logger.error(errorMsg)
             response.status_code = HTTPStatus.BAD_REQUEST.value
             return errorMsg
diff --git a/src/isar/mission_planner/mission_validator.py b/src/isar/mission_planner/mission_validator.py
index a438774c..aa3498f9 100644
--- a/src/isar/mission_planner/mission_validator.py
+++ b/src/isar/mission_planner/mission_validator.py
@@ -1,15 +1,17 @@
-from typing import List
+from typing import List, Tuple
 
 from isar.models.mission import Mission
 
 
 def is_robot_capable_of_mission(
     mission: Mission, robot_capabilities: List[str]
-) -> bool:
-    return all(
-        [
-            step.type in robot_capabilities
-            for task in mission.tasks
-            for step in task.steps
-        ]
-    )
+) -> Tuple[bool, List[str]]:
+    success: bool = True
+    missing_capabilities: List[str] = []
+    for task in mission.tasks:
+        for step in task.steps:
+            if not step.type in robot_capabilities:
+                success = False
+                missing_capabilities.append(step.type)
+
+    return success, missing_capabilities
diff --git a/tests/isar/mission_planner/test_mission_validator.py b/tests/isar/mission_planner/test_mission_validator.py
index 7956b397..c556131f 100644
--- a/tests/isar/mission_planner/test_mission_validator.py
+++ b/tests/isar/mission_planner/test_mission_validator.py
@@ -12,7 +12,7 @@
     ],
 )
 def test_is_robot_capable_of_mission(mission, capabilities, expected_return) -> None:
-    return_value: bool = is_robot_capable_of_mission(
+    return_value, missing_capabilities = is_robot_capable_of_mission(
         mission=mission, robot_capabilities=capabilities
     )
     assert return_value == expected_return