From 6afdaba09d4e23c8eb048173b55f1002b2ee0523 Mon Sep 17 00:00:00 2001 From: fukimiyazaki Date: Sat, 2 Mar 2024 09:57:20 +0900 Subject: [PATCH] call pause navigation before call auto navigation Signed-off-by: fukimiyazaki --- cabot_ui/cabot_ui/navigation.py | 13 ++++++------- cabot_ui/scripts/cabot_ui_manager.py | 12 +++++++----- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/cabot_ui/cabot_ui/navigation.py b/cabot_ui/cabot_ui/navigation.py index 4e0948b9..2b3af77a 100644 --- a/cabot_ui/cabot_ui/navigation.py +++ b/cabot_ui/cabot_ui/navigation.py @@ -627,14 +627,13 @@ def _auto_navigation(self, callback): self._exist_elevator_goal = False self._start_manual_floor = None index = next((i for i, goal in enumerate(self._sub_goals) if isinstance(goal, navgoal.ElevatorOutGoal)), None) - if index is None: - self._sub_goals.insert(0, self._current_goal) - elif index != 0: + self._logger.info(F"auto_navigation index : {index}") + if index == 1: + self._sub_goals.pop(0) + elif index > 2: del self._sub_goals[:index + 1] - else: - return - self._logger.info(F"auto navigation sub_goals : {self._sub_goals}") - self._navigate_next_sub_goal() + self._logger.info(F"auto_navigation sub_goals : {self._sub_goals}") + self._navigate_next_sub_goal() # wrap execution by a queue def cancel_navigation(self, callback=None): diff --git a/cabot_ui/scripts/cabot_ui_manager.py b/cabot_ui/scripts/cabot_ui_manager.py index 0f081f4f..99dd6b65 100755 --- a/cabot_ui/scripts/cabot_ui_manager.py +++ b/cabot_ui/scripts/cabot_ui_manager.py @@ -375,10 +375,10 @@ def _process_navigation_event(self, event): self._interface.set_pause_control(True) self._navigation.set_pause_control(True) if self._status_manager.state == State.in_pause: - self._logger.info("NavigationState: changing to manual (user)") - self._status_manager.set_state(State.in_manual) - self._navigation.manual_navigation() - self._logger.info("NavigationState: changed (user)") + self._logger.info("NavigationState: changing to manual (user)") + self._status_manager.set_state(State.in_manual) + self._navigation.manual_navigation() + self._logger.info("NavigationState: changed (user)") # operations depents on the current navigation state if self._status_manager.state == State.in_preparation: @@ -504,7 +504,9 @@ def done_callback(): self._logger.info("NavigationState: resuming (user)") self._interface.resume_navigation() self._status_manager.set_state(State.in_action) - self._navigation.auto_navigation() + def done_callback(): + self._navigation.auto_navigation() + self._navigation.pause_navigation(done_callback) self._logger.info("NavigationState: resumed (user)") else: self._logger.info("NavigationState: state is not in pause state")