From d92d7e35340931e4eb4e47b1fe756534a4a551e9 Mon Sep 17 00:00:00 2001 From: Leonardo Wellausen Date: Wed, 26 Jun 2024 18:11:03 +0300 Subject: [PATCH 1/5] Cancel mission on STOP --- task_manager/task_manager/tasks/mission.py | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/task_manager/task_manager/tasks/mission.py b/task_manager/task_manager/tasks/mission.py index 6f8b060..49d4445 100644 --- a/task_manager/task_manager/tasks/mission.py +++ b/task_manager/task_manager/tasks/mission.py @@ -81,15 +81,13 @@ 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 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. + if goal_handle.is_cancel_requested: 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 +100,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, From 00e4d8ebcf7b89299b1f5a544765bddc2d74cda3 Mon Sep 17 00:00:00 2001 From: Leonardo Wellausen Date: Wed, 26 Jun 2024 18:42:48 +0300 Subject: [PATCH 2/5] Adding comment --- task_manager/task_manager/tasks/mission.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/task_manager/task_manager/tasks/mission.py b/task_manager/task_manager/tasks/mission.py index 49d4445..445f299 100644 --- a/task_manager/task_manager/tasks/mission.py +++ b/task_manager/task_manager/tasks/mission.py @@ -87,7 +87,7 @@ def execute_cb(self, goal_handle: ServerGoalHandle) -> MissionAction.Result: if goal_handle.is_cancel_requested: goal_handle.canceled() - else: + else: # If mission is not cancelled but a subtask fail, we abort the mission goal_handle.abort() return result From 39c23038550078255deda1bb8728f1768c3d6d9b Mon Sep 17 00:00:00 2001 From: Leonardo Wellausen Date: Mon, 1 Jul 2024 15:44:37 +0300 Subject: [PATCH 3/5] Fixing tests and changes from PR --- task_manager/task_manager/tasks/mission.py | 9 +++++++-- task_manager/test/test_mission.py | 7 ++++--- 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/task_manager/task_manager/tasks/mission.py b/task_manager/task_manager/tasks/mission.py index 445f299..5743f92 100644 --- a/task_manager/task_manager/tasks/mission.py +++ b/task_manager/task_manager/tasks/mission.py @@ -17,6 +17,7 @@ import uuid from typing import Callable +from icecream import ic # ROS from rclpy.action.server import ActionServer, CancelResponse, ServerGoalHandle from rclpy.callback_groups import ReentrantCallbackGroup @@ -85,9 +86,13 @@ def execute_cb(self, goal_handle: ServerGoalHandle) -> MissionAction.Result: mission_result.skipped = True continue - if goal_handle.is_cancel_requested: + # 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: goal_handle.canceled() - else: # If mission is not cancelled but a subtask fail, we abort the mission + else: goal_handle.abort() return result diff --git a/task_manager/test/test_mission.py b/task_manager/test/test_mission.py index 4d29721..57a8500 100644 --- a/task_manager/test/test_mission.py +++ b/task_manager/test/test_mission.py @@ -85,7 +85,7 @@ def test_mission_not_successful(self): 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 +97,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_handle = Mock(request=request) + mock_handle.is_cancel_requested = False + result = self.mission.execute_cb(goal_handle=mock_handle) self.assertEqual(result, expected_result) From f5458c8df7921821934b3d9e27a887b289a08225 Mon Sep 17 00:00:00 2001 From: Leonardo Wellausen Date: Mon, 1 Jul 2024 16:50:58 +0300 Subject: [PATCH 4/5] Adding new tests --- task_manager/task_manager/tasks/mission.py | 1 - task_manager/test/test_mission.py | 32 ++++++++++++++++++++-- 2 files changed, 29 insertions(+), 4 deletions(-) diff --git a/task_manager/task_manager/tasks/mission.py b/task_manager/task_manager/tasks/mission.py index 5743f92..833d47a 100644 --- a/task_manager/task_manager/tasks/mission.py +++ b/task_manager/task_manager/tasks/mission.py @@ -17,7 +17,6 @@ import uuid from typing import Callable -from icecream import ic # ROS from rclpy.action.server import ActionServer, CancelResponse, ServerGoalHandle from rclpy.callback_groups import ReentrantCallbackGroup diff --git a/task_manager/test/test_mission.py b/task_manager/test/test_mission.py index 57a8500..13fa3e2 100644 --- a/task_manager/test/test_mission.py +++ b/task_manager/test/test_mission.py @@ -84,6 +84,32 @@ 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 status of the subtasks are set correctly when the subtasks fail or are cancelled, or if the + Mission is cancelled.""" + 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 though a subtask is aborted, no error is raised when the task is allowed to be skipped.""" request = MissionAction.Goal() @@ -97,9 +123,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") ] - mock_handle = Mock(request=request) - mock_handle.is_cancel_requested = False - result = self.mission.execute_cb(goal_handle=mock_handle) + 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) From eb8bc9f08f80a2ba0d4e2c4c5cd4524a369c3b0a Mon Sep 17 00:00:00 2001 From: Leonardo Wellausen Date: Mon, 1 Jul 2024 17:03:46 +0300 Subject: [PATCH 5/5] Fixes docstring --- task_manager/test/test_mission.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/task_manager/test/test_mission.py b/task_manager/test/test_mission.py index 13fa3e2..27a3714 100644 --- a/task_manager/test/test_mission.py +++ b/task_manager/test/test_mission.py @@ -85,8 +85,7 @@ def test_mission_not_successful(self): self.assertEqual(result.mission_results[0].task_status, TaskStatus.ERROR) def test_mission_not_successful_skipping_task(self): - """Tests that the status of the subtasks are set correctly when the subtasks fail or are cancelled, or if the - Mission is cancelled.""" + """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="{}")] )