fix bug and skip turn goal

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

cabot_ui/cabot_ui/interface.py

@@ -195,9 +195,9 @@ 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 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")

cabot_ui/cabot_ui/navgoal.py

@@ -665,7 +665,10 @@ def enter(self):
         CaBotRclpyUtil.info(F"cab poi      {str(self.cab_poi.local_geometry)}")
         pose = geoutil.Pose.pose_from_points(self.cab_poi.door_geometry, self.delegate.current_pose)
         CaBotRclpyUtil.info(F"turn target {str(pose)}")
-        self.delegate.turn_towards(pose.orientation, self.goal_handle_callback, self.done_callback)
+        if self.delegate.is_manual:
+            self.done_callback(True)
+        else:
+            self.delegate.turn_towards(pose.orientation, self.goal_handle_callback, self.done_callback)
 
     def done_callback(self, result):
         if result:
@@ -707,7 +710,10 @@ def enter(self):
         CaBotRclpyUtil.info("call turn_towards")
         pose = geoutil.Pose(x=self.cab_poi.x, y=self.cab_poi.y, r=self.cab_poi.r)
         CaBotRclpyUtil.info(F"turn target {str(pose)}")
-        self.delegate.turn_towards(pose.orientation, self.goal_handle_callback, self.done_callback, clockwise=-1)
+        if self.delegate.is_manual:
+            self.done_callback(True)
+        else:
+            self.delegate.turn_towards(pose.orientation, self.goal_handle_callback, self.done_callback, clockwise=-1)
 
     def done_callback(self, result):
         if result:

cabot_ui/cabot_ui/navigation.py

@@ -320,6 +320,7 @@ def __init__(self, node: Node, datautil_instance=None, anchor_file=None, wait_fo
 
         self._exist_elevator_goal = False
         self._start_manual_floor = None
+        self.is_manual = False
 
         self._max_speed = node.declare_parameter("max_speed", 1.1).value
         self._max_acc = node.declare_parameter("max_acc", 0.3).value
@@ -598,15 +599,17 @@ def manual_navigation(self, callback=None):
     def _manual_navigation(self, callback):
         self._logger.info(F"navigation.{util.callee_name()} called")
         self.delegate.activity_log("cabot/navigation", "manual")
+        self._navigate_next_sub_goal()
 
+        self.is_manual = True
         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
+        self._logger.info(F"manual navigation start_manual_floor : {self._start_manual_floor}")
 
     # wrap execution by a queue
     def auto_navigation(self, callback=None):
@@ -616,15 +619,21 @@ def _auto_navigation(self, callback):
         self._logger.info(F"navigation.{util.callee_name()} called")
         self.delegate.activity_log("cabot/navigation", "auto")
 
+        self.is_manual = False
         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
+            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:
+                del self._sub_goals[:index + 1]
+            else:
+                return
+            self._logger.info(F"auto navigation sub_goals : {self._sub_goals}")
             self._navigate_next_sub_goal()
 
     # wrap execution by a queue

cabot_ui/scripts/cabot_ui_manager.py

@@ -375,9 +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: resuming (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: