skip elevator during stopping navigation

Signed-off-by: fukimiyazaki <[email protected]>

cabot_ui/cabot_ui/interface.py

@@ -195,6 +195,10 @@ def in_preparation(self):
         self._activity_log("cabot/interface", "status", "prepare")
         self.speak(i18n.localized_string("IN_PRERARATION"))
 
+    def in_preparation(self): #TODO
+        self._activity_log("cabot/interface", "status", "manual")
+        # self.speak(i18n.localized_string("IN_PRERARATION"))
+
     def i_am_ready(self):
         self._activity_log("cabot/interface", "status", "ready")
         self.speak(i18n.localized_string("I_AM_READY"))

cabot_ui/cabot_ui/navigation.py

@@ -318,6 +318,9 @@ def __init__(self, node: Node, datautil_instance=None, anchor_file=None, wait_fo
         self.pause_control_loop_handler = None
         self.lock = threading.Lock()
 
+        self._exist_elevator_goal = False
+        self._start_manual_floor = None
+
         self._max_speed = node.declare_parameter("max_speed", 1.1).value
         self._max_acc = node.declare_parameter("max_acc", 0.3).value
 
@@ -588,6 +591,42 @@ def _resume_navigation(self, callback):
         self.delegate.activity_log("cabot/navigation", "resume")
         self._navigate_next_sub_goal()
 
+    # wrap execution by a queue
+    def manual_navigation(self, callback=None):
+        self._process_queue.append((self._manual_navigation, callback))
+
+    def _manual_navigation(self, callback):
+        self._logger.info(F"navigation.{util.callee_name()} called")
+        self.delegate.activity_log("cabot/navigation", "manual")
+
+        self._logger.info(F"manual navigation sub_goals : {self._sub_goals}")
+        if any(isinstance(goal, navgoal.ElevatorOutGoal) for goal in self._sub_goals):
+            self._exist_elevator_goal = True
+        else:
+            return
+
+        self._logger.info(F"manual navigation start_manual_floor : {self._start_manual_floor}")
+        self._start_manual_floor = self.current_floor
+
+    # wrap execution by a queue
+    def auto_navigation(self, callback=None):
+        self._process_queue.append((self._auto_navigation, callback))
+
+    def _auto_navigation(self, callback):
+        self._logger.info(F"navigation.{util.callee_name()} called")
+        self.delegate.activity_log("cabot/navigation", "auto")
+
+        self._logger.info(F"auto_navigation exist_elevator_goal : {self._exist_elevator_goal}")
+        self._logger.info(F"auto_navigation start_manual_floor : {self._start_manual_floor}")
+        self._logger.info(F"auto_navigation current_floor : {self.current_floor}")
+        if self._exist_elevator_goal and (self._start_manual_floor != self.current_floor):
+            index = next((i for i, goal in enumerate(self._sub_goals) if isinstance(goal, navgoal.ElevatorOutGoal)), None)
+            del self._sub_goals[:index + 1]
+            self._logger.info(F"auto navigation sub_goals : {self._sub_goals}")
+            self._exist_elevator_goal = False
+            self._start_manual_floor = None
+            self._navigate_next_sub_goal()
+
     # wrap execution by a queue
     def cancel_navigation(self, callback=None):
         self._process_queue.append((self._cancel_navigation, callback))

cabot_ui/cabot_ui/status.py

@@ -36,6 +36,8 @@ class State(Enum):
     in_pausing = 4
     # pausing action to wait user's decision
     in_pause = 5
+    # manual navigation
+    in_manual = 6
 
     # transition from in_action to in_pause
     # waiting_pause = 4

cabot_ui/scripts/cabot_ui_manager.py

@@ -374,6 +374,10 @@ def _process_navigation_event(self, event):
             self._logger.info("NavigationState: Pause control = True")
             self._interface.set_pause_control(True)
             self._navigation.set_pause_control(True)
+            if self._status_manager.state == State.in_pause:
+                    self._logger.info("NavigationState: resuming (user)")
+                    self._status_manager.set_state(State.in_manual)
+                    self._navigation.manual_navigation()
 
         # operations depents on the current navigation state
         if self._status_manager.state == State.in_preparation:
@@ -495,6 +499,12 @@ def done_callback():
                     self._status_manager.set_state(State.in_action)
                     self._navigation.resume_navigation()
                     self._logger.info("NavigationState: resumed (user)")
+                elif self._status_manager.state == State.in_manual:
+                    self._logger.info("NavigationState: resuming (user)")
+                    self._interface.resume_navigation()
+                    self._status_manager.set_state(State.in_action)
+                    self._navigation.auto_navigation()
+                    self._logger.info("NavigationState: resumed (user)")
                 else:
                     self._logger.info("NavigationState: state is not in pause state")
             else: