From c9c52633855fac3ad94102c528dab03d99e734fd Mon Sep 17 00:00:00 2001 From: Ramon Wijnands Date: Tue, 11 Jun 2024 16:28:36 +0200 Subject: [PATCH] Fix bug that uses the status of a previous goal The `_goal_status` member was not set after sending or accepting a goal. So the cancel timeout check would check against the status of a previous goal and then conclude the goal is already finished. --- smach_ros/smach_ros/simple_action_state.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/smach_ros/smach_ros/simple_action_state.py b/smach_ros/smach_ros/simple_action_state.py index 40501f2..a458522 100644 --- a/smach_ros/smach_ros/simple_action_state.py +++ b/smach_ros/smach_ros/simple_action_state.py @@ -130,7 +130,7 @@ def __init__(self, self._action_spec = action_spec # Set timeouts - self._goal_status = 0 + self._goal_status = GoalStatus.STATUS_UNKNOWN self._goal_result = None self._exec_timeout = exec_timeout self._preempt_timeout = preempt_timeout @@ -354,6 +354,7 @@ def execute(self, ud): # Activate the state before sending the goal self._activate_time = self.node.get_clock().now() self._status = ActionState.ACTIVE + self._goal_status = GoalStatus.STATUS_UNKNOWN with self._done_cond: # Dispatch goal via non-blocking call to action server @@ -440,6 +441,7 @@ def _goal_accepted_cb(self, future): Accept or reject a client request to begin an action. """ goal_handle = future.result() + self._goal_status = goal_handle.status if not goal_handle.accepted: self.node.get_logger().debug(f"Goal request from action {self._action_name} has been rejected!")