call pause navigation before call auto navigation

Signed-off-by: fukimiyazaki <[email protected]>
CMU-cabot · Mar 2, 2024 · 6afdaba · 6afdaba
1 parent b303c6b
commit 6afdaba
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 12 deletions.
diff --git 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
@@ -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")