Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 import rospy
00031 import threading
00032
00033 from actionlib_msgs.msg import GoalStatus
00034 from actionlib.action_client import ActionClient, CommState, get_name_of_constant
00035
00036
00037 class SimpleGoalState:
00038 PENDING = 0
00039 ACTIVE = 1
00040 DONE = 2
00041
00042
00043 SimpleGoalState.to_string = classmethod(get_name_of_constant)
00044
00045
00046 class SimpleActionClient:
00047
00048
00049
00050
00051
00052
00053
00054 def __init__(self, ns, ActionSpec):
00055 self.action_client = ActionClient(ns, ActionSpec)
00056 self.simple_state = SimpleGoalState.DONE
00057 self.gh = None
00058 self.done_condition = threading.Condition()
00059
00060
00061
00062
00063
00064
00065
00066 def wait_for_server(self, timeout=rospy.Duration()):
00067 return self.action_client.wait_for_server(timeout)
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083 def send_goal(self, goal, done_cb=None, active_cb=None, feedback_cb=None):
00084
00085 self.stop_tracking_goal()
00086
00087 self.done_cb = done_cb
00088 self.active_cb = active_cb
00089 self.feedback_cb = feedback_cb
00090
00091 self.simple_state = SimpleGoalState.PENDING
00092 self.gh = self.action_client.send_goal(goal, self._handle_transition, self._handle_feedback)
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109 def send_goal_and_wait(self, goal, execute_timeout=rospy.Duration(), preempt_timeout=rospy.Duration()):
00110 self.send_goal(goal)
00111 if not self.wait_for_result(execute_timeout):
00112
00113 rospy.logdebug("Canceling goal")
00114 self.cancel_goal()
00115 if self.wait_for_result(preempt_timeout):
00116 rospy.logdebug("Preempt finished within specified preempt_timeout [%.2f]", preempt_timeout.to_sec())
00117 else:
00118 rospy.logdebug("Preempt didn't finish specified preempt_timeout [%.2f]", preempt_timeout.to_sec())
00119 return self.get_state()
00120
00121
00122
00123
00124 def wait_for_result(self, timeout=rospy.Duration()):
00125 if not self.gh:
00126 rospy.logerr("Called wait_for_goal_to_finish when no goal exists")
00127 return False
00128
00129 timeout_time = rospy.get_rostime() + timeout
00130 loop_period = rospy.Duration(0.1)
00131 with self.done_condition:
00132 while not rospy.is_shutdown():
00133 time_left = timeout_time - rospy.get_rostime()
00134 if timeout > rospy.Duration(0.0) and time_left <= rospy.Duration(0.0):
00135 break
00136
00137 if self.simple_state == SimpleGoalState.DONE:
00138 break
00139
00140 if time_left > loop_period or timeout == rospy.Duration():
00141 time_left = loop_period
00142
00143 self.done_condition.wait(time_left.to_sec())
00144
00145 return self.simple_state == SimpleGoalState.DONE
00146
00147
00148 def get_result(self):
00149 if not self.gh:
00150 rospy.logerr("Called get_result when no goal is running")
00151 return None
00152
00153 return self.gh.get_result()
00154
00155
00156
00157
00158
00159
00160
00161
00162 def get_state(self):
00163 if not self.gh:
00164 rospy.logerr("Called get_state when no goal is running")
00165 return GoalStatus.LOST
00166 status = self.gh.get_goal_status()
00167
00168 if status == GoalStatus.RECALLING:
00169 status = GoalStatus.PENDING
00170 elif status == GoalStatus.PREEMPTING:
00171 status = GoalStatus.ACTIVE
00172
00173 return status
00174
00175
00176
00177
00178
00179
00180
00181 def get_goal_status_text(self):
00182 if not self.gh:
00183 rospy.logerr("Called get_goal_status_text when no goal is running")
00184 return "ERROR: Called get_goal_status_text when no goal is running"
00185
00186 return self.gh.get_goal_status_text()
00187
00188
00189
00190
00191
00192 def cancel_all_goals(self):
00193 self.action_client.cancel_all_goals()
00194
00195
00196
00197
00198
00199
00200 def cancel_goals_at_and_before_time(self, time):
00201 self.action_client.cancel_goals_at_and_before_time(time)
00202
00203
00204 def cancel_goal(self):
00205 if self.gh:
00206 self.gh.cancel()
00207
00208
00209
00210
00211
00212 def stop_tracking_goal(self):
00213 self.gh = None
00214
00215 def _handle_transition(self, gh):
00216 comm_state = gh.get_comm_state()
00217
00218 error_msg = "Received comm state %s when in simple state %s with SimpleActionClient in NS %s" % \
00219 (CommState.to_string(comm_state), SimpleGoalState.to_string(self.simple_state), rospy.resolve_name(self.action_client.ns))
00220
00221 if comm_state == CommState.ACTIVE:
00222 if self.simple_state == SimpleGoalState.PENDING:
00223 self._set_simple_state(SimpleGoalState.ACTIVE)
00224 if self.active_cb:
00225 self.active_cb()
00226 elif self.simple_state == SimpleGoalState.DONE:
00227 rospy.logerr(error_msg)
00228 elif comm_state == CommState.RECALLING:
00229 if self.simple_state != SimpleGoalState.PENDING:
00230 rospy.logerr(error_msg)
00231 elif comm_state == CommState.PREEMPTING:
00232 if self.simple_state == SimpleGoalState.PENDING:
00233 self._set_simple_state(SimpleGoalState.ACTIVE)
00234 if self.active_cb:
00235 self.active_cb()
00236 elif self.simple_state == SimpleGoalState.DONE:
00237 rospy.logerr(error_msg)
00238 elif comm_state == CommState.DONE:
00239 if self.simple_state in [SimpleGoalState.PENDING, SimpleGoalState.ACTIVE]:
00240 self._set_simple_state(SimpleGoalState.DONE)
00241 if self.done_cb:
00242 self.done_cb(gh.get_goal_status(), gh.get_result())
00243 with self.done_condition:
00244 self.done_condition.notifyAll()
00245 elif self.simple_state == SimpleGoalState.DONE:
00246 rospy.logerr("SimpleActionClient received DONE twice")
00247
00248 def _handle_feedback(self, gh, feedback):
00249 if not self.gh:
00250
00251
00252
00253 return
00254 if gh != self.gh:
00255 rospy.logerr("Got a feedback callback on a goal handle that we're not tracking. %s vs %s" %
00256 (self.gh.comm_state_machine.action_goal.goal_id.id, gh.comm_state_machine.action_goal.goal_id.id))
00257 return
00258 if self.feedback_cb:
00259 self.feedback_cb(feedback)
00260
00261 def _set_simple_state(self, state):
00262 self.simple_state = state