Cancel Mission on STOP (#18)

task_manager/task_manager/tasks/mission.py

@@ -81,15 +81,17 @@ def execute_cb(self, goal_handle: ServerGoalHandle) -> MissionAction.Result:
             mission_result.skipped = False
 
             if subtask_result.task_status != TaskStatus.DONE:
-                if subtask.allow_skipping:
+                if subtask.allow_skipping and not goal_handle.is_cancel_requested:
                     mission_result.skipped = True
                     continue
 
+                # If the subtask has been cancelled together with the mission, we cancel the mission
+                # If the subtask has been cancelled/failed/indefinitely in progress, we abort the mission
+                # If the mission has been cancelled but the subtask is failed/indefinitely in progress,
+                # we abort the mission
                 if subtask_result.task_status == TaskStatus.CANCELED and goal_handle.is_cancel_requested:
-                    # Need to also check if the cancel was requested. If the goal was cancelled
-                    # through a system task, we cannot set the status to be cancelled and must abort instead.
                     goal_handle.canceled()
-                else:  # Could be ERROR or IN_PROGRESS if the goal couldn't be cancelled
+                else:
                     goal_handle.abort()
                 return result
 
@@ -102,7 +104,7 @@ def get_task_specs(mission_topic) -> TaskSpecs:
         return TaskSpecs(
             task_name="system/mission",
             blocking=False,
-            cancel_on_stop=False,
+            cancel_on_stop=True,
             topic=mission_topic,
             cancel_reported_as_success=False,
             reentrant=False,

task_manager/test/test_mission.py

@@ -84,8 +84,33 @@ def test_mission_not_successful(self):
             mock_goal_handle.abort.assert_called_once()
             self.assertEqual(result.mission_results[0].task_status, TaskStatus.ERROR)
 
+    def test_mission_not_successful_skipping_task(self):
+        """Tests that the mission is properly cancelled or aborted when a subtask is skipped."""
+        request = MissionAction.Goal(
+            subtasks=[SubtaskGoal(task_name="test/mock_subtask", allow_skipping=True, task_data="{}")]
+        )
+        mock_goal_handle = Mock(request=request)
+        mock_goal_handle.is_cancel_requested = True
+
+        with self.subTest("mission_canceled"):
+            self.mission.execute_task_cb.return_value = ExecuteTask.Result(
+                task_status=TaskStatus.CANCELED, task_result="{}"
+            )
+            result = self.mission.execute_cb(goal_handle=mock_goal_handle)
+            mock_goal_handle.canceled.assert_called_once()
+            self.assertEqual(result.mission_results[0].task_status, TaskStatus.CANCELED)
+
+        mock_goal_handle.reset_mock()
+        with self.subTest("mission_error"):
+            self.mission.execute_task_cb.return_value = ExecuteTask.Result(
+                task_status=TaskStatus.ERROR, task_result="{}"
+            )
+            result = self.mission.execute_cb(goal_handle=mock_goal_handle)
+            mock_goal_handle.abort.assert_called_once()
+            self.assertEqual(result.mission_results[0].task_status, TaskStatus.ERROR)
+
     def test_skipping_subtask(self):
-        """Tests that even tho a subtask is aborted, no error is raised when the task is allowed to be skipped."""
+        """Tests that even though a subtask is aborted, no error is raised when the task is allowed to be skipped."""
         request = MissionAction.Goal()
         request.subtasks = [
             SubtaskGoal(task_name="test/mock_subtask", task_data="{}", allow_skipping=True, task_id="123")
@@ -97,8 +122,9 @@ def test_skipping_subtask(self):
         expected_result.mission_results = [
             SubtaskResult(task_name="test/mock_subtask", task_status=TaskStatus.ERROR, skipped=True, task_id="123")
         ]
-
-        result = self.mission.execute_cb(goal_handle=Mock(request=request))
+        mock_goal_handle = Mock(request=request)
+        mock_goal_handle.is_cancel_requested = False
+        result = self.mission.execute_cb(goal_handle=mock_goal_handle)
         self.assertEqual(result, expected_result)