add timeout to execute done_callback if future does not respond becau… (

#65) * add timeout to execute done_callback if future does not respond because bt_navigator has died Signed-off-by: fukimiyazaki <[email protected]> * - do not auto repeat when it is pausing - retry sending goal if bt_navigator is not available (timeout) - reduce bond_timeout from 60 to 15 Signed-off-by: Daisuke Sato <[email protected]> * fix line errors and typo Signed-off-by: Daisuke Sato <[email protected]> --------- Signed-off-by: fukimiyazaki <[email protected]> Signed-off-by: Daisuke Sato <[email protected]> Co-authored-by: Daisuke Sato <[email protected]>
CMU-cabot · Aug 26, 2024 · 162f825 · 162f825
1 parent f223f6b
commit 162f825
Show file tree

Hide file tree

Showing 7 changed files with 64 additions and 10 deletions.
diff --git a/cabot_navigation2/launch/bringup_launch.py b/cabot_navigation2/launch/bringup_launch.py
@@ -210,7 +210,7 @@ def generate_launch_description():
                     output='log',
                     parameters=[{'use_sim_time': use_sim_time},
                                 {'autostart': autostart},
-                                {'bond_timeout': 60.0},
+                                {'bond_timeout': 15.0},
                                 {'node_names': ['controller_server',
                                                 'planner_server',
                                                 'behavior_server',
@@ -283,7 +283,7 @@ def generate_launch_description():
                     namespace='local',
                     parameters=[{'use_sim_time': use_sim_time},
                                 {'autostart': autostart},
-                                {'bond_timeout': 60.0},
+                                {'bond_timeout': 15.0},
                                 {'node_names': ['controller_server',
                                                 'planner_server',
                                                 'behavior_server',
@@ -309,7 +309,7 @@ def generate_launch_description():
             output='log',
             parameters=[{'use_sim_time': use_sim_time},
                         {'autostart': autostart},
-                        {'bond_timeout': 60.0},
+                        {'bond_timeout': 15.0},
                         {'node_names': ['map_server']},
                         ]
         ),

diff --git a/cabot_navigation2/test/run_test.py b/cabot_navigation2/test/run_test.py
@@ -597,6 +597,11 @@ def topic_callback(msg):
         case['done'] = True
         case['success'] = None
 
+        def cancel_func():
+            logger.debug(F"cancel {case}")
+            self.cancel_subscription(case)
+        return cancel_func
+
     @wait_test(1)
     def check_topic_error(self, case, test_action):
         logger.debug(f"{callee_name()} {test_action}")

diff --git a/cabot_ui/cabot_ui/interface.py b/cabot_ui/cabot_ui/interface.py
@@ -188,6 +188,10 @@ def resume_navigation(self):
         self._activity_log("cabot/interface", "navigation", "resume")
         self.speak(i18n.localized_string("RESUME_NAVIGATION"))
 
+    def pausing_navigation(self):
+        self._activity_log("cabot/interface", "navigation", "pausing")
+        self.speak(i18n.localized_string("PLEASE_WAIT_FOR_A_SECOND"))
+
     def start_exploration(self):
         pass  # self.speak(i18n.localized_string("START_EXPLORATION"))
 

diff --git a/cabot_ui/cabot_ui/navgoal.py b/cabot_ui/cabot_ui/navgoal.py
@@ -22,6 +22,7 @@
 import math
 import inspect
 import numpy
+import threading
 import time
 import traceback
 import yaml
@@ -570,11 +571,31 @@ def _cancel(self, callback=None):
             future = handle.cancel_goal_async()
 
             def done_callback(future):
+                if future.cancelled():
+                    self._logger.info("Future was cancelled.")
+                elif future.done():
+                    error = future.exception()
+                    if error:
+                        self._logger.error(f"Future resulted in error: {error}")
+                    else:
+                        self._logger.info(f"cancel future result = {future.result}")
                 if cancel_callback:
                     cancel_callback()
-                self._logger.info(f"cancel future result = {future.result}")
                 self.delegate._process_queue.append((self.cancel, callback))
             future.add_done_callback(done_callback)
+
+            def timeout_watcher(future, timeout_duration):
+                start_time = time.time()
+                while not future.done():
+                    if time.time() - start_time > timeout_duration:
+                        self._logger.error("Timeout occurred while waiting for future to complete")
+                        future.cancel()
+                        return
+                    time.sleep(0.1)
+
+            timeout_tread = threading.Thread(target=timeout_watcher, args=(future, 5))
+            timeout_tread.start()
+
             self._logger.info(f"sent cancel goal: {len(self._handles)} handles remaining")
         else:
             if callback:
@@ -836,8 +857,11 @@ def done_callback(self, future):
             self.prevent_callback = False
             return
 
-        CaBotRclpyUtil.info(F"NavGoal completed result={future.result()}, {self.route_index}/{len(self.navcog_routes)}")
-        status = future.result().status
+        if future:
+            CaBotRclpyUtil.info(F"NavGoal completed result={future.result()}, {self.route_index}/{len(self.navcog_routes)}")
+        else:
+            CaBotRclpyUtil.info("Could not send goal")
+        status = future.result().status if future is not None else None
 
         # TODO(daisuke): needs to change test case conditions
         if status == GoalStatus.STATUS_SUCCEEDED:

diff --git a/cabot_ui/cabot_ui/navigation.py b/cabot_ui/cabot_ui/navigation.py
@@ -635,9 +635,8 @@ def _retry_navigation(self):
         self.delegate.activity_log("cabot/navigation", "retry")
         self.turns = []
 
-        if self._current_goal:
-            self._goal_index -= 1
-            self._navigate_next_sub_goal()
+        self._logger.info(f"{self._current_goal=}, {self._goal_index}")
+        self._navigate_next_sub_goal()
 
     # wrap execution by a queue
     def pause_navigation(self, callback):
@@ -1058,6 +1057,7 @@ def _check_goal(self, current_pose):
             self.delegate.activity_log("cabot/navigation", "goal_canceled", F"{goal.__class__.__name__}")
             self._stop_loop()
             self._current_goal = None
+            self._goal_index = max(-1, self._goal_index - 1)
             return
 
         if not goal.is_completed:
@@ -1127,9 +1127,27 @@ def navigate_to_pose(self, goal_pose, behavior_tree, gh_cb, done_cb, namespace="
         self._logger.info("add done callback")
         future.add_done_callback(lambda future: self._navigate_to_pose_sent_goal(goal, future, gh_cb, done_cb))
         self._logger.info("added done callback")
+
+        def timeout_watcher(future, timeout_duration):
+            start_time = time.time()
+            while not future.done():
+                if time.time() - start_time > timeout_duration:
+                    self._logger.error("Timeout occurred while waiting for sending goal future to be completed")
+                    future.cancel()
+                    return
+                time.sleep(0.1)
+
+        timeout_tread = threading.Thread(target=timeout_watcher, args=(future, 5))
+        timeout_tread.start()
         return future
 
     def _navigate_to_pose_sent_goal(self, goal, future, gh_cb, done_cb):
+        if future.cancelled():
+            try:
+                done_cb(None)
+            except:  # noqa: E722
+                self._logger.error(traceback.format_exc())
+            return
         self._logger.info("_navigate_to_pose_sent_goal")
         self._logger.info(F"navigate to pose, threading.get_ident {threading.get_ident()}")
         goal_handle = future.result()

diff --git a/cabot_ui/scripts/cabot_ui_manager.py b/cabot_ui/scripts/cabot_ui_manager.py
@@ -340,6 +340,7 @@ def _process_menu_event(self, event):
     def _process_navigation_event(self, event):
         if event.type != NavigationEvent.TYPE:
             return
+        self._logger.info(f"_process_navigation_event {event}")
 
         # operations indepent from the navigation state
         if event.subtype == "language":
@@ -498,6 +499,8 @@ 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_pausing:
+                    self._interface.pausing_navigation()
                 else:
                     self._logger.info("NavigationState: state is not in pause state")
             else:

diff --git a/script/run_test.sh b/script/run_test.sh
@@ -40,7 +40,7 @@ function help()
     echo "-w          wait ready"
 }
 
-blue "Start runnint tests"
+blue "Start running tests"
 
 pwd=`pwd`
 scriptdir=`dirname $0`