workaround to avoid infinite wait when ActionClient fails to get goal…

… handle

Signed-off-by: Daisuke Sato <[email protected]>

cabot_bt/behavior_trees/navigate_w_replanning_and_recovery.xml

@@ -90,13 +90,7 @@
             </RateController>>
 
             <!-- follow path -->
-            <ReactiveFallback>
-              <!-- if the goal is updated, abort following first, this will be recorvered by RecoveryComputePathRepeat -->
-              <GoalUpdated/>
-              <Sequence>
-                <FollowPath path="{path}" controller_id="FollowPath" goal_checker_id="cabot_goal_checker"/>
-              </Sequence>
-            </ReactiveFallback>
+            <FollowPath path="{path}" controller_id="FollowPath" goal_checker_id="cabot_goal_checker"/>
           </PipelineSequence>
 
           <!-- if follow path is aborted, compute path and try again -->

cabot_ui/cabot_ui/navgoal.py

@@ -539,11 +539,14 @@ def enter(self):
         # so we use navigate_to_pose and planner will listen the published path
         # self.delegate.navigate_through_poses(self.ros_path.poses[1:], NavGoal.DEFAULT_BT_XML, self.done_callback)
         self.handle = self.delegate.navigate_to_pose(self.ros_path.poses[-1], NavGoal.DEFAULT_BT_XML, self.done_callback)
+        if self.handle is None:
+            self._is_canceled = True
 
     def done_callback(self, future):
         if self.prevent_callback:
             self.prevent_callback = False
             return
+
         CaBotRclpyUtil.info(F"NavGoal completed result={future.result()}")
         status = future.result().status
         self._is_completed = (status == GoalStatus.STATUS_SUCCEEDED)
@@ -644,6 +647,8 @@ def enter(self):
         super(ElevatorInGoal, self).enter()
         # use odom frame for navigation
         self.handle = self.delegate.navigate_to_pose(self.to_pose_stamped_msg(frame_id=self.global_map_name), ElevatorGoal.ELEVATOR_BT_XML, self.done_callback)
+        if self.handle is None:
+            self._is_canceled = True
 
     def done_callback(self, future):
         CaBotRclpyUtil.info("ElevatorInGoal completed")
@@ -712,6 +717,9 @@ def enter(self):
         self.delegate.publish_path(path, False)
 
         self.handle = self.delegate.navigate_to_pose(end, ElevatorGoal.LOCAL_ODOM_BT_XML, self.done_callback, namespace='/local')
+        if self.handle is None:
+            self._is_canceled = True
+
 
     def done_callback(self, future):
         CaBotRclpyUtil.info("ElevatorOutGoal completed")
@@ -793,6 +801,9 @@ def narrow_enter(self, bt):
         # basically the same as a NavGoal, use BT_XML that makes the footprint the same as an elevator to pass through narrow spaces
         # self.delegate.navigate_through_poses(self.ros_path.poses[1:], NavGoal.DEFAULT_BT_XML, self.done_callback)
         self.handle = self.delegate.navigate_to_pose(self.ros_path.poses[-1], bt, self.done_callback)
+        if self.handle is None:
+            self._is_canceled = True
+
 
     @util.setInterval(5, times=1)
     def wait_for_announce(self):
@@ -904,6 +915,9 @@ def enter(self):
             self.handle = self.delegate.navigate_to_pose(self.to_pose_stamped_msg(frame_id=self.global_map_name), QueueNavGoal.QUEUE_EXIT_BT_XML, self.done_callback)
         else:
             self.handle = self.delegate.navigate_to_pose(self.to_pose_stamped_msg(frame_id=self.global_map_name), QueueNavGoal.QUEUE_BT_XML, self.done_callback)
+        if self.handle is None:
+            self._is_canceled = True
+
 
     def done_callback(self, future):
         self._is_completed = future.done()

cabot_ui/cabot_ui/navigation.py

@@ -336,6 +336,7 @@ def __init__(self, node: Node, datautil_instance=None, anchor_file=None, wait_fo
         set_social_distance_topic = node.declare_parameter("set_social_distance_topic", "/set_social_distance").value
         self.set_social_distance_pub = node.create_publisher(geometry_msgs.msg.Point, set_social_distance_topic, 10, callback_group=MutuallyExclusiveCallbackGroup())
 
+        self._process_queue = []
         self._start_loop()
 
     def _localize_status_callback(self, msg):
@@ -452,6 +453,9 @@ def _goal_updated_callback(self, msg):
     # public interfaces
 
     def set_destination(self, destination):
+        self._process_queue.append((self._set_destination, destination))
+
+    def _set_destination(self, destination):
         """
         memo: current logic is not beautiful.
         1. get a NavCog route from the server
@@ -487,6 +491,9 @@ def set_destination(self, destination):
         self._navigate_next_sub_goal()
 
     def retry_navigation(self):
+        self._process_queue.append((self._retry_navigation,))
+
+    def _retry_navigation(self):
         self._logger.info(F"navigation.{util.callee_name()} called")
         self.delegate.activity_log("cabot/navigation", "retry")
         self.turns = []
@@ -578,6 +585,7 @@ def _start_loop(self):
             self.lock.release()
 
     def _stop_loop(self):
+        return
         self._logger.info(F"navigation.{util.callee_name()} called")
         if self.lock.acquire():
             if self._loop_handle is not None:
@@ -594,6 +602,11 @@ def _check_loop(self):
             self._stop_loop()
             return
 
+        if len(self._process_queue) > 0:
+            process = self._process_queue.pop(0)
+            process[0](*process[1:])
+            return
+
         # need a robot position
         try:
             self.current_pose = self.current_local_pose()
@@ -869,9 +882,13 @@ def unblock(future):
         future.add_done_callback(unblock)
         self._logger.info("sending goal")
         self._logger.info(F"navigate to pose, threading.get_ident {threading.get_ident()}")
-        event.wait()
-        self._logger.info(F"sent goal: {goal}")
+        event.wait(timeout=2)
         goal_handle = future.result()
+        if goal_handle is None:
+            self._logger.error(F"could not send goal in time, it might get an warning")
+            return
+
+        self._logger.info(F"sent goal: {goal}")
         self._logger.info(F"get goal handle {goal_handle}")
         get_result_future = goal_handle.get_result_async()
         self._logger.info("add done callback")
@@ -883,57 +900,6 @@ def unblock(future):
         self.visualizer.visualize()
         return goal_handle
 
-    def navigate_through_poses(self, goal_poses, behavior_tree, done_cb, namespace=""):
-        self._logger.info(F"{namespace}/navigate_through_poses")
-        self.delegate.activity_log("cabot/navigation", "navigate_through_pose")
-        client = self._clients["/".join([namespace, "navigate_through_poses"])]
-        goal = nav2_msgs.action.NavigateThroughPoses.Goal()
-
-        if behavior_tree.startswith("package://"):
-            start = len("package://")
-            end = behavior_tree.find("/", len("pacakge://"))
-            package = behavior_tree[start:end]
-            behavior_tree = get_package_share_directory(package) + behavior_tree[end:]
-            self._logger.info(F"package={package}, behavior_tree={behavior_tree}")
-
-        goal.behavior_tree = behavior_tree
-
-        if namespace == "":
-            goal.poses = []
-            for pose in goal_poses:
-                t_pose = self.buffer.transform(pose, self._global_map_name)
-                t_pose.pose.position.z = 0
-                t_pose.header.stamp = self._node.get_clock().now().to_msg()
-                t_pose.header.frame_id = self._global_map_name
-                goal.poses.append(t_pose)
-        elif namespace == "local":
-            goal.poses = []
-            for pose in goal_poses:
-                t_pose = pose
-                t_pose.header.stamp = self._node.get_clock().now().to_msg()
-                t_pose.header.frame_id = "local/odom"
-                goal.poses.append(t_pose)
-        else:
-            self._logger.info(F"unknown namespace {namespace}")
-            return
-
-        future = client.send_goal_async(goal)
-        event = threading.Event()
-
-        def unblock(future):
-            self._logger.info("_unblock is called")
-            event.set()
-
-        future.add_done_callback(unblock)
-        event.wait()
-        goal_handle = future.result()
-        get_result_future = goal_handle.get_result_async()
-        get_result_future.add_done_callback(done_cb)
-
-        self.visualizer.reset()
-        self.visualizer.goal = goal
-        self.visualizer.visualize()
-        self._logger.info(F"sent goal {goal}")
 
     def turn_towards(self, orientation, callback, clockwise=0):
         self._logger.info("turn_towards")
@@ -963,11 +929,16 @@ def unblock(future):
                 event.set()
 
             future.add_done_callback(unblock)
-            event.wait()
+            event.wait(timeout=2)
             goal_handle = future.result()
+            if goal_handle is None:
+                self._logger.error(F"turn_towards: could not send goal in time, it might get an warning")
+                return
+
+            self._logger.info(F"sent goal: {goal}")
+            self._logger.info(F"get goal handle {goal_handle}")
             get_result_future = goal_handle.get_result_async()
             get_result_future.add_done_callback(lambda f: self.turn_towards(orientation, callback, clockwise=clockwise))
-            self._logger.info(F"sent goal {goal}")
 
             # add position and use quaternion to visualize
             # self.visualizer.goal = goal

cabot_ui/scripts/cabot_ui_manager.py

@@ -181,8 +181,8 @@ def goal_canceled(self, goal):
     def _retry_navigation(self):
         self._retry_count += 1
         self._logger.info("NavigationState: retrying (system)")
-        self._navigation.retry_navigation()
         self._status_manager.set_state(State.in_action)
+        self._navigation.retry_navigation()
         self._logger.info("NavigationState: retried (system)")
 
     def have_arrived(self, goal):
@@ -359,7 +359,6 @@ def _process_navigation_event(self, event):
             self._logger.info(F"Destination: {event.param}")
             self._logger.info(F"process event threading.get_ident {threading.get_ident()}")
             self._retry_count = 0
-            self._navigation.set_destination(event.param)
             self.destination = event.param
             # change handle mode
             request = std_srvs.srv.SetBool.Request()
@@ -381,10 +380,10 @@ def _process_navigation_event(self, event):
             # change state
             # change to waiting_action by using actionlib
             self._status_manager.set_state(State.in_action)
+            self._navigation.set_destination(event.param)
 
         if event.subtype == "summons":
             self._logger.info(F"Summons Destination: {event.param}")
-            self._navigation.set_destination(event.param)
             self.destination = event.param
             # change handle mode
             request = std_srvs.srv.SetBool.Request()
@@ -406,6 +405,7 @@ def _process_navigation_event(self, event):
             # change state
             # change to waiting_action by using actionlib
             self._status_manager.set_state(State.in_summons)
+            self._navigation.set_destination(event.param)
 
         if event.subtype == "event":
             self._navigation.process_event(event)