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_result 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 return GoalStatus.LOST
00165 status = self.gh.get_goal_status()
00166
00167 if status == GoalStatus.RECALLING:
00168 status = GoalStatus.PENDING
00169 elif status == GoalStatus.PREEMPTING:
00170 status = GoalStatus.ACTIVE
00171
00172 return status
00173
00174
00175
00176
00177
00178
00179
00180 def get_goal_status_text(self):
00181 if not self.gh:
00182 rospy.logerr("Called get_goal_status_text when no goal is running")
00183 return "ERROR: Called get_goal_status_text when no goal is running"
00184
00185 return self.gh.get_goal_status_text()
00186
00187
00188
00189
00190
00191 def cancel_all_goals(self):
00192 self.action_client.cancel_all_goals()
00193
00194
00195
00196
00197
00198
00199 def cancel_goals_at_and_before_time(self, time):
00200 self.action_client.cancel_goals_at_and_before_time(time)
00201
00202
00203 def cancel_goal(self):
00204 if self.gh:
00205 self.gh.cancel()
00206
00207
00208
00209
00210
00211 def stop_tracking_goal(self):
00212 self.gh = None
00213
00214 def _handle_transition(self, gh):
00215 comm_state = gh.get_comm_state()
00216
00217 error_msg = "Received comm state %s when in simple state %s with SimpleActionClient in NS %s" % \
00218 (CommState.to_string(comm_state), SimpleGoalState.to_string(self.simple_state), rospy.resolve_name(self.action_client.ns))
00219
00220 if comm_state == CommState.ACTIVE:
00221 if self.simple_state == SimpleGoalState.PENDING:
00222 self._set_simple_state(SimpleGoalState.ACTIVE)
00223 if self.active_cb:
00224 self.active_cb()
00225 elif self.simple_state == SimpleGoalState.DONE:
00226 rospy.logerr(error_msg)
00227 elif comm_state == CommState.RECALLING:
00228 if self.simple_state != SimpleGoalState.PENDING:
00229 rospy.logerr(error_msg)
00230 elif comm_state == CommState.PREEMPTING:
00231 if self.simple_state == SimpleGoalState.PENDING:
00232 self._set_simple_state(SimpleGoalState.ACTIVE)
00233 if self.active_cb:
00234 self.active_cb()
00235 elif self.simple_state == SimpleGoalState.DONE:
00236 rospy.logerr(error_msg)
00237 elif comm_state == CommState.DONE:
00238 if self.simple_state in [SimpleGoalState.PENDING, SimpleGoalState.ACTIVE]:
00239 self._set_simple_state(SimpleGoalState.DONE)
00240 if self.done_cb:
00241 self.done_cb(gh.get_goal_status(), gh.get_result())
00242 with self.done_condition:
00243 self.done_condition.notifyAll()
00244 elif self.simple_state == SimpleGoalState.DONE:
00245 rospy.logerr("SimpleActionClient received DONE twice")
00246
00247 def _handle_feedback(self, gh, feedback):
00248 if not self.gh:
00249
00250
00251
00252 return
00253 if gh != self.gh:
00254 rospy.logerr("Got a feedback callback on a goal handle that we're not tracking. %s vs %s" %
00255 (self.gh.comm_state_machine.action_goal.goal_id.id, gh.comm_state_machine.action_goal.goal_id.id))
00256 return
00257 if self.feedback_cb:
00258 self.feedback_cb(feedback)
00259
00260 def _set_simple_state(self, state):
00261 self.simple_state = state