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
00031 import threading
00032 import time
00033 import rospy
00034 from rospy import Header
00035 from actionlib_msgs.msg import *
00036 from action_client import ActionClient, CommState, get_name_of_constant
00037
00038 class SimpleGoalState:
00039 PENDING = 0
00040 ACTIVE = 1
00041 DONE = 2
00042 SimpleGoalState.to_string = classmethod(get_name_of_constant)
00043
00044
00045 class SimpleActionClient:
00046
00047
00048
00049
00050
00051
00052
00053 def __init__(self, ns, ActionSpec):
00054 self.action_client = ActionClient(ns, ActionSpec)
00055 self.simple_state = SimpleGoalState.DONE
00056 self.gh = None
00057 self.done_condition = threading.Condition()
00058
00059
00060
00061
00062
00063
00064
00065 def wait_for_server(self, timeout = rospy.Duration()):
00066 return self.action_client.wait_for_server(timeout)
00067
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
00110 def send_goal_and_wait(self, goal, execute_timeout = rospy.Duration(), preempt_timeout = rospy.Duration()):
00111 self.send_goal(goal)
00112 if not self.wait_for_result(execute_timeout):
00113
00114 rospy.logdebug("Canceling goal")
00115 self.cancel_goal()
00116 if self.wait_for_result(preempt_timeout):
00117 rospy.logdebug("Preempt finished within specified preempt_timeout [%.2f]", preempt_timeout.to_sec());
00118 else:
00119 rospy.logdebug("Preempt didn't finish specified preempt_timeout [%.2f]", preempt_timeout.to_sec());
00120 return self.get_state()
00121
00122
00123
00124
00125
00126 def wait_for_result(self, timeout = rospy.Duration()):
00127 if not self.gh:
00128 rospy.logerr("Called wait_for_goal_to_finish when no goal exists")
00129 return False
00130
00131 timeout_time = rospy.get_rostime() + timeout
00132 loop_period = rospy.Duration(0.1)
00133 with self.done_condition:
00134 while not rospy.is_shutdown():
00135 time_left = timeout_time - rospy.get_rostime()
00136 if timeout > rospy.Duration(0.0) and time_left <= rospy.Duration(0.0):
00137 break
00138
00139 if self.simple_state == SimpleGoalState.DONE:
00140 break
00141
00142 if time_left > loop_period or timeout == rospy.Duration():
00143 time_left = loop_period
00144
00145 self.done_condition.wait(time_left.to_sec())
00146
00147 return self.simple_state == SimpleGoalState.DONE
00148
00149
00150
00151 def get_result(self):
00152 if not self.gh:
00153 rospy.logerr("Called get_result when no goal is running")
00154 return None
00155
00156 return self.gh.get_result()
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166 def get_state(self):
00167 if not self.gh:
00168 rospy.logerr("Called get_state when no goal is running")
00169 return GoalStatus.LOST
00170 status = self.gh.get_goal_status()
00171
00172 if status == GoalStatus.RECALLING:
00173 status = GoalStatus.PENDING
00174 elif status == GoalStatus.PREEMPTING:
00175 status = GoalStatus.ACTIVE
00176
00177 return status
00178
00179
00180
00181
00182
00183
00184
00185
00186 def get_goal_status_text(self):
00187 if not self.gh:
00188 rospy.logerr("Called get_goal_status_text when no goal is running")
00189 return "ERROR: Called get_goal_status_text when no goal is running"
00190
00191 return self.gh.get_goal_status_text()
00192
00193
00194
00195
00196
00197
00198
00199
00200 def cancel_all_goals(self):
00201 self.action_client.cancel_all_goals()
00202
00203
00204
00205
00206
00207
00208
00209
00210 def cancel_goals_at_and_before_time(self, time):
00211 self.action_client.cancel_goals_at_and_before_time(time)
00212
00213
00214
00215 def cancel_goal(self):
00216 if self.gh:
00217 self.gh.cancel()
00218
00219
00220
00221
00222
00223
00224 def stop_tracking_goal(self):
00225 self.gh = None
00226
00227 def _handle_transition(self, gh):
00228 comm_state = gh.get_comm_state()
00229
00230 error_msg = "Received comm state %s when in simple state %s with SimpleActionClient in NS %s" % \
00231 (CommState.to_string(comm_state), SimpleGoalState.to_string(self.simple_state), rospy.resolve_name(self.action_client.ns))
00232
00233 if comm_state == CommState.ACTIVE:
00234 if self.simple_state == SimpleGoalState.PENDING:
00235 self._set_simple_state(SimpleGoalState.ACTIVE)
00236 if self.active_cb:
00237 self.active_cb()
00238 elif self.simple_state == SimpleGoalState.DONE:
00239 rospy.logerr(error_msg)
00240 elif comm_state == CommState.RECALLING:
00241 if self.simple_state != SimpleGoalState.PENDING:
00242 rospy.logerr(error_msg)
00243 elif comm_state == CommState.PREEMPTING:
00244 if self.simple_state == SimpleGoalState.PENDING:
00245 self._set_simple_state(SimpleGoalState.ACTIVE)
00246 if self.active_cb:
00247 self.active_cb()
00248 elif self.simple_state == SimpleGoalState.DONE:
00249 rospy.logerr(error_msg)
00250 elif comm_state == CommState.DONE:
00251 if self.simple_state in [SimpleGoalState.PENDING, SimpleGoalState.ACTIVE]:
00252 self._set_simple_state(SimpleGoalState.DONE)
00253 if self.done_cb:
00254 self.done_cb(gh.get_goal_status(), gh.get_result())
00255 with self.done_condition:
00256 self.done_condition.notifyAll()
00257 elif self.simple_state == SimpleGoalState.DONE:
00258 rospy.logerr("SimpleActionClient received DONE twice")
00259
00260 def _handle_feedback(self, gh, feedback):
00261 if not self.gh:
00262 rospy.logerr("Got a feedback callback when we're not tracking a goal. (id: %s)" % \
00263 gh.comm_state_machine.action_goal.goal_id.id)
00264 return
00265 if gh != self.gh:
00266 rospy.logerr("Got a feedback callback on a goal handle that we're not tracking. %s vs %s" % \
00267 (self.gh.comm_state_machine.action_goal.goal_id.id,
00268 gh.comm_state_machine.action_goal.goal_id.id))
00269 return
00270 if self.feedback_cb:
00271 self.feedback_cb(feedback)
00272
00273
00274 def _set_simple_state(self, state):
00275 self.simple_state = state