From 3118728c96c9c125fb6e00d95b64f0edf39787da Mon Sep 17 00:00:00 2001 From: kn Date: Mon, 2 Jan 2023 14:19:06 +0100 Subject: [PATCH] fix 206: introduce lock to prevent send_goal race condition --- actionlib/src/actionlib/simple_action_client.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/actionlib/src/actionlib/simple_action_client.py b/actionlib/src/actionlib/simple_action_client.py index a8c133d2..be97a433 100644 --- a/actionlib/src/actionlib/simple_action_client.py +++ b/actionlib/src/actionlib/simple_action_client.py @@ -29,7 +29,6 @@ # Author: Stuart Glaser import rospy import threading - from actionlib_msgs.msg import GoalStatus from actionlib.action_client import ActionClient, CommState, get_name_of_constant @@ -56,6 +55,7 @@ def __init__(self, ns, ActionSpec): self.simple_state = SimpleGoalState.DONE self.gh = None self.done_condition = threading.Condition() + self.lock = threading.RLock() # fix #206 ## @brief Blocks until the action server connects to this client ## @@ -89,7 +89,8 @@ def send_goal(self, goal, done_cb=None, active_cb=None, feedback_cb=None): self.feedback_cb = feedback_cb self.simple_state = SimpleGoalState.PENDING - self.gh = self.action_client.send_goal(goal, self._handle_transition, self._handle_feedback) + with self.lock: + self.gh = self.action_client.send_goal(goal, self._handle_transition, self._handle_feedback) ## @brief Sends a goal to the ActionServer, waits for the goal to complete, and preempts goal is necessary ## @@ -213,9 +214,10 @@ def stop_tracking_goal(self): def _handle_transition(self, gh): - if gh != self.gh: - rospy.logerr("Got a transition callback on a goal handle that we're not tracking") - return + with self.lock: + if gh != self.gh: + rospy.logerr("Got a transition callback on a goal handle that we're not tracking") + return comm_state = gh.get_comm_state()