-
Notifications
You must be signed in to change notification settings - Fork 154
/
action_client.py
642 lines (545 loc) · 26.8 KB
/
action_client.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
#! /usr/bin/env python
# Copyright (c) 2009, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution.
# * Neither the name of the Willow Garage, Inc. nor the names of its
# contributors may be used to endorse or promote products derived from
# this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
# Author: Stuart Glaser
'''
Example:
from move_base.msg import *
rospy.init_node('foo')
from move_base.msg import *
from geometry_msgs.msg import *
g1 = MoveBaseGoal(PoseStamped(Header(frame_id = 'base_link'),
Pose(Point(2, 0, 0),
Quaternion(0, 0, 0, 1))))
g2 = MoveBaseGoal(PoseStamped(Header(frame_id = 'base_link'),
Pose(Point(5, 0, 0),
Quaternion(0, 0, 0, 1))))
client = ActionClient('move_base', MoveBaseAction)
h1 = client.send_goal(g1)
h2 = client.send_goal(g2)
client.cancel_all_goals()
'''
import threading
import weakref
import time
import rospy
from rospy import Header
from actionlib_msgs.msg import GoalID, GoalStatus, GoalStatusArray
from actionlib.exceptions import ActionException
g_goal_id = 1
def get_name_of_constant(C, n):
for k, v in C.__dict__.items():
if isinstance(v, int) and v == n:
return k
return "NO_SUCH_STATE_%d" % n
class CommState(object):
WAITING_FOR_GOAL_ACK = 0
PENDING = 1
ACTIVE = 2
WAITING_FOR_RESULT = 3
WAITING_FOR_CANCEL_ACK = 4
RECALLING = 5
PREEMPTING = 6
DONE = 7
LOST = 8
class TerminalState(object):
RECALLED = GoalStatus.RECALLED
REJECTED = GoalStatus.REJECTED
PREEMPTED = GoalStatus.PREEMPTED
ABORTED = GoalStatus.ABORTED
SUCCEEDED = GoalStatus.SUCCEEDED
LOST = GoalStatus.LOST
GoalStatus.to_string = classmethod(get_name_of_constant)
CommState.to_string = classmethod(get_name_of_constant)
TerminalState.to_string = classmethod(get_name_of_constant)
def _find_status_by_goal_id(status_array, id):
for s in status_array.status_list:
if s.goal_id.id == id:
return s
return None
## @brief Client side handle to monitor goal progress.
##
## A ClientGoalHandle is a reference counted object that is used to
## manipulate and monitor the progress of an already dispatched
## goal. Once all the goal handles go out of scope (or are reset), an
## ActionClient stops maintaining state for that goal.
class ClientGoalHandle:
## @brief Internal use only
##
## ClientGoalHandle objects should be created by the action
## client. You should never need to construct one yourself.
def __init__(self, comm_state_machine):
self.comm_state_machine = comm_state_machine
# print "GH created. id = %.3f" % self.comm_state_machine.action_goal.goal_id.stamp.to_sec()
## @brief True iff the two ClientGoalHandle's are tracking the same goal
def __eq__(self, o):
if not o:
return False
return self.comm_state_machine == o.comm_state_machine
## @brief True iff the two ClientGoalHandle's are tracking different goals
def __ne__(self, o):
if not o:
return True
return not (self.comm_state_machine == o.comm_state_machine)
## @brieft Hash function for ClientGoalHandle
def __hash__(self):
return hash(self.comm_state_machine)
## @brief Sends a cancel message for this specific goal to the ActionServer.
##
## Also transitions the client state to WAITING_FOR_CANCEL_ACK
def cancel(self):
with self.comm_state_machine.mutex:
cancel_msg = GoalID(stamp=rospy.Time(0),
id=self.comm_state_machine.action_goal.goal_id.id)
self.comm_state_machine.send_cancel_fn(cancel_msg)
self.comm_state_machine.transition_to(CommState.WAITING_FOR_CANCEL_ACK)
## @brief Get the state of this goal's communication state machine from interaction with the server
##
## Possible States are: WAITING_FOR_GOAL_ACK, PENDING, ACTIVE, WAITING_FOR_RESULT,
## WAITING_FOR_CANCEL_ACK, RECALLING, PREEMPTING, DONE
##
## @return The current goal's communication state with the server
def get_comm_state(self):
if not self.comm_state_machine:
rospy.logerr("Trying to get_comm_state on an inactive ClientGoalHandle.")
return CommState.LOST
return self.comm_state_machine.state
## @brief Returns the current status of the goal.
##
## Possible states are listed in the enumeration in the
## actionlib_msgs/GoalStatus message.
##
## @return The current status of the goal.
def get_goal_status(self):
if not self.comm_state_machine:
rospy.logerr("Trying to get_goal_status on an inactive ClientGoalHandle.")
return GoalStatus.PENDING
return self.comm_state_machine.latest_goal_status.status
## @brief Returns the current status text of the goal.
##
## The text is sent by the action server.
##
## @return The current status text of the goal.
def get_goal_status_text(self):
if not self.comm_state_machine:
rospy.logerr("Trying to get_goal_status_text on an inactive ClientGoalHandle.")
return "ERROR: Trying to get_goal_status_text on an inactive ClientGoalHandle."
return self.comm_state_machine.latest_goal_status.text
## @brief Gets the result produced by the action server for this goal.
##
## @return None if no result was receieved. Otherwise the goal's result as a *Result message.
def get_result(self):
if not self.comm_state_machine:
rospy.logerr("Trying to get_result on an inactive ClientGoalHandle.")
return None
if not self.comm_state_machine.latest_result:
# rospy.logerr("Trying to get_result on a ClientGoalHandle when no result has been received.")
return None
return self.comm_state_machine.latest_result.result
## @brief Gets the terminal state information for this goal.
##
## Possible States Are: RECALLED, REJECTED, PREEMPTED, ABORTED, SUCCEEDED, LOST
## This call only makes sense if CommState==DONE. This will send ROS_WARNs if we're not in DONE
##
## @return The terminal state as an integer from the GoalStatus message.
def get_terminal_state(self):
if not self.comm_state_machine:
rospy.logerr("Trying to get_terminal_state on an inactive ClientGoalHandle.")
return GoalStatus.LOST
with self.comm_state_machine.mutex:
if self.comm_state_machine.state != CommState.DONE:
rospy.logwarn("Asking for the terminal state when we're in [%s]",
CommState.to_string(self.comm_state_machine.state))
goal_status = self.comm_state_machine.latest_goal_status.status
if goal_status in [GoalStatus.PREEMPTED, GoalStatus.SUCCEEDED,
GoalStatus.ABORTED, GoalStatus.REJECTED,
GoalStatus.RECALLED, GoalStatus.LOST]:
return goal_status
rospy.logerr("Asking for a terminal state, but the goal status is %d", goal_status)
return GoalStatus.LOST
NO_TRANSITION = -1
INVALID_TRANSITION = -2
_transitions = {
CommState.WAITING_FOR_GOAL_ACK: {
GoalStatus.PENDING: CommState.PENDING,
GoalStatus.ACTIVE: CommState.ACTIVE,
GoalStatus.REJECTED: (CommState.PENDING, CommState.WAITING_FOR_RESULT),
GoalStatus.RECALLING: (CommState.PENDING, CommState.RECALLING),
GoalStatus.RECALLED: (CommState.PENDING, CommState.WAITING_FOR_RESULT),
GoalStatus.PREEMPTED: (CommState.ACTIVE, CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
GoalStatus.SUCCEEDED: (CommState.ACTIVE, CommState.WAITING_FOR_RESULT),
GoalStatus.ABORTED: (CommState.ACTIVE, CommState.WAITING_FOR_RESULT),
GoalStatus.PREEMPTING: (CommState.ACTIVE, CommState.PREEMPTING)},
CommState.PENDING: {
GoalStatus.PENDING: NO_TRANSITION,
GoalStatus.ACTIVE: CommState.ACTIVE,
GoalStatus.REJECTED: CommState.WAITING_FOR_RESULT,
GoalStatus.RECALLING: CommState.RECALLING,
GoalStatus.RECALLED: (CommState.RECALLING, CommState.WAITING_FOR_RESULT),
GoalStatus.PREEMPTED: (CommState.ACTIVE, CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
GoalStatus.SUCCEEDED: (CommState.ACTIVE, CommState.WAITING_FOR_RESULT),
GoalStatus.ABORTED: (CommState.ACTIVE, CommState.WAITING_FOR_RESULT),
GoalStatus.PREEMPTING: (CommState.ACTIVE, CommState.PREEMPTING)},
CommState.ACTIVE: {
GoalStatus.PENDING: INVALID_TRANSITION,
GoalStatus.ACTIVE: NO_TRANSITION,
GoalStatus.REJECTED: INVALID_TRANSITION,
GoalStatus.RECALLING: INVALID_TRANSITION,
GoalStatus.RECALLED: INVALID_TRANSITION,
GoalStatus.PREEMPTED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
GoalStatus.SUCCEEDED: CommState.WAITING_FOR_RESULT,
GoalStatus.ABORTED: CommState.WAITING_FOR_RESULT,
GoalStatus.PREEMPTING: CommState.PREEMPTING},
CommState.WAITING_FOR_RESULT: {
GoalStatus.PENDING: INVALID_TRANSITION,
GoalStatus.ACTIVE: NO_TRANSITION,
GoalStatus.REJECTED: NO_TRANSITION,
GoalStatus.RECALLING: INVALID_TRANSITION,
GoalStatus.RECALLED: NO_TRANSITION,
GoalStatus.PREEMPTED: NO_TRANSITION,
GoalStatus.SUCCEEDED: NO_TRANSITION,
GoalStatus.ABORTED: NO_TRANSITION,
GoalStatus.PREEMPTING: INVALID_TRANSITION},
CommState.WAITING_FOR_CANCEL_ACK: {
GoalStatus.PENDING: NO_TRANSITION,
GoalStatus.ACTIVE: NO_TRANSITION,
GoalStatus.REJECTED: CommState.WAITING_FOR_RESULT,
GoalStatus.RECALLING: CommState.RECALLING,
GoalStatus.RECALLED: (CommState.RECALLING, CommState.WAITING_FOR_RESULT),
GoalStatus.PREEMPTED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
GoalStatus.SUCCEEDED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
GoalStatus.ABORTED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
GoalStatus.PREEMPTING: CommState.PREEMPTING},
CommState.RECALLING: {
GoalStatus.PENDING: INVALID_TRANSITION,
GoalStatus.ACTIVE: INVALID_TRANSITION,
GoalStatus.REJECTED: CommState.WAITING_FOR_RESULT,
GoalStatus.RECALLING: NO_TRANSITION,
GoalStatus.RECALLED: CommState.WAITING_FOR_RESULT,
GoalStatus.PREEMPTED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
GoalStatus.SUCCEEDED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
GoalStatus.ABORTED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
GoalStatus.PREEMPTING: CommState.PREEMPTING},
CommState.PREEMPTING: {
GoalStatus.PENDING: INVALID_TRANSITION,
GoalStatus.ACTIVE: INVALID_TRANSITION,
GoalStatus.REJECTED: INVALID_TRANSITION,
GoalStatus.RECALLING: INVALID_TRANSITION,
GoalStatus.RECALLED: INVALID_TRANSITION,
GoalStatus.PREEMPTED: CommState.WAITING_FOR_RESULT,
GoalStatus.SUCCEEDED: CommState.WAITING_FOR_RESULT,
GoalStatus.ABORTED: CommState.WAITING_FOR_RESULT,
GoalStatus.PREEMPTING: NO_TRANSITION},
CommState.DONE: {
GoalStatus.PENDING: INVALID_TRANSITION,
GoalStatus.ACTIVE: INVALID_TRANSITION,
GoalStatus.REJECTED: NO_TRANSITION,
GoalStatus.RECALLING: INVALID_TRANSITION,
GoalStatus.RECALLED: NO_TRANSITION,
GoalStatus.PREEMPTED: NO_TRANSITION,
GoalStatus.SUCCEEDED: NO_TRANSITION,
GoalStatus.ABORTED: NO_TRANSITION,
GoalStatus.PREEMPTING: INVALID_TRANSITION}}
class CommStateMachine:
def __init__(self, action_goal, transition_cb, feedback_cb, send_goal_fn, send_cancel_fn):
self.action_goal = action_goal
self.transition_cb = transition_cb
self.feedback_cb = feedback_cb
self.send_goal_fn = send_goal_fn
self.send_cancel_fn = send_cancel_fn
self.state = CommState.WAITING_FOR_GOAL_ACK
self.mutex = threading.RLock()
self.latest_goal_status = GoalStatus(status=GoalStatus.PENDING)
self.latest_result = None
def __eq__(self, o):
return self.action_goal.goal_id.id == o.action_goal.goal_id.id
## @brieft Hash function for CommStateMachine
def __hash__(self):
return hash(self.action_goal.goal_id.id)
def set_state(self, state):
rospy.logdebug("Transitioning CommState from %s to %s",
CommState.to_string(self.state), CommState.to_string(state))
self.state = state
##
## @param gh ClientGoalHandle
## @param status_array actionlib_msgs/GoalStatusArray
def update_status(self, status_array):
with self.mutex:
if self.state == CommState.DONE:
return
status = _find_status_by_goal_id(status_array, self.action_goal.goal_id.id)
# You mean you haven't heard of me?
if not status:
if self.state not in [CommState.WAITING_FOR_GOAL_ACK,
CommState.WAITING_FOR_RESULT,
CommState.DONE]:
self._mark_as_lost()
return
self.latest_goal_status = status
# Determines the next state from the lookup table
if self.state not in _transitions:
rospy.logerr("CommStateMachine is in a funny state: %i" % self.state)
return
if status.status not in _transitions[self.state]:
rospy.logerr("Got an unknown status from the ActionServer: %i" % status.status)
return
next_state = _transitions[self.state][status.status]
# Knowing the next state, what should we do?
if next_state == NO_TRANSITION:
pass
elif next_state == INVALID_TRANSITION:
rospy.logerr("Invalid goal status transition from %s to %s" %
(CommState.to_string(self.state), GoalStatus.to_string(status.status)))
else:
if hasattr(next_state, '__getitem__'):
for s in next_state:
self.transition_to(s)
else:
self.transition_to(next_state)
def transition_to(self, state):
rospy.logdebug("Transitioning to %s (from %s, goal: %s)",
CommState.to_string(state), CommState.to_string(self.state),
self.action_goal.goal_id.id)
self.state = state
if self.transition_cb:
self.transition_cb(ClientGoalHandle(self))
def _mark_as_lost(self):
self.latest_goal_status.status = GoalStatus.LOST
self.transition_to(CommState.DONE)
def update_result(self, action_result):
# Might not be for us
if self.action_goal.goal_id.id != action_result.status.goal_id.id:
return
with self.mutex:
self.latest_goal_status = action_result.status
self.latest_result = action_result
if self.state in [CommState.WAITING_FOR_GOAL_ACK,
CommState.WAITING_FOR_CANCEL_ACK,
CommState.PENDING,
CommState.ACTIVE,
CommState.WAITING_FOR_RESULT,
CommState.RECALLING,
CommState.PREEMPTING]:
# Stuffs the goal status in the result into a GoalStatusArray
status_array = GoalStatusArray()
status_array.status_list.append(action_result.status)
self.update_status(status_array)
self.transition_to(CommState.DONE)
elif self.state == CommState.DONE:
rospy.logerr("Got a result when we were already in the DONE state")
else:
rospy.logerr("In a funny state: %i" % self.state)
def update_feedback(self, action_feedback):
# Might not be for us
if self.action_goal.goal_id.id != action_feedback.status.goal_id.id:
return
# with self.mutex:
if self.feedback_cb and self.state != CommState.DONE:
self.feedback_cb(ClientGoalHandle(self), action_feedback.feedback)
class GoalManager:
# statuses - a list of weak references to CommStateMachine objects
def __init__(self, ActionSpec):
self.list_mutex = threading.RLock()
self.statuses = []
self.send_goal_fn = None
try:
a = ActionSpec()
self.ActionSpec = ActionSpec
self.ActionGoal = type(a.action_goal)
self.ActionResult = type(a.action_result)
self.ActionFeedback = type(a.action_feedback)
except AttributeError:
raise ActionException("Type is not an action spec: %s" % str(ActionSpec))
def _generate_id(self):
global g_goal_id
id, g_goal_id = g_goal_id, g_goal_id + 1
now = rospy.Time.now()
return GoalID(id="%s-%i-%.3f" % (rospy.get_caller_id(), id, now.to_sec()), stamp=now)
def register_send_goal_fn(self, fn):
self.send_goal_fn = fn
def register_cancel_fn(self, fn):
self.cancel_fn = fn
## Sends off a goal and starts tracking its status.
##
## @return ClientGoalHandle for the sent goal.
def init_goal(self, goal, transition_cb=None, feedback_cb=None):
action_goal = self.ActionGoal(header=Header(),
goal_id=self._generate_id(),
goal=goal)
action_goal.header.stamp = rospy.get_rostime()
csm = CommStateMachine(action_goal, transition_cb, feedback_cb,
self.send_goal_fn, self.cancel_fn)
with self.list_mutex:
self.statuses.append(weakref.ref(csm))
self.send_goal_fn(action_goal)
return ClientGoalHandle(csm)
# Pulls out the statuses that are still live (creating strong
# references to them)
def _get_live_statuses(self):
with self.list_mutex:
live_statuses = [r() for r in self.statuses]
live_statuses = [x for x in live_statuses if x]
return live_statuses
## Updates the statuses of all goals from the information in status_array.
##
## @param status_array (\c actionlib_msgs/GoalStatusArray)
def update_statuses(self, status_array):
with self.list_mutex:
# Garbage collects dead status objects
self.statuses = [r for r in self.statuses if r()]
for status in self._get_live_statuses():
status.update_status(status_array)
def update_results(self, action_result):
for status in self._get_live_statuses():
status.update_result(action_result)
def update_feedbacks(self, action_feedback):
for status in self._get_live_statuses():
status.update_feedback(action_feedback)
class ActionClient:
## @brief Constructs an ActionClient and opens connections to an ActionServer.
##
## @param ns The namespace in which to access the action. For
## example, the "goal" topic should occur under ns/goal
##
## @param ActionSpec The *Action message type. The ActionClient
## will grab the other message types from this type.
def __init__(self, ns, ActionSpec):
self.ns = ns
self.last_status_msg = None
try:
a = ActionSpec()
self.ActionSpec = ActionSpec
self.ActionGoal = type(a.action_goal)
self.ActionResult = type(a.action_result)
self.ActionFeedback = type(a.action_feedback)
except AttributeError:
raise ActionException("Type is not an action spec: %s" % str(ActionSpec))
self.pub_queue_size = rospy.get_param('actionlib_client_pub_queue_size', 10)
if self.pub_queue_size < 0:
self.pub_queue_size = 10
self.pub_goal = rospy.Publisher(rospy.remap_name(ns) + '/goal', self.ActionGoal, queue_size=self.pub_queue_size)
self.pub_cancel = rospy.Publisher(rospy.remap_name(ns) + '/cancel', GoalID, queue_size=self.pub_queue_size)
self.manager = GoalManager(ActionSpec)
self.manager.register_send_goal_fn(self.pub_goal.publish)
self.manager.register_cancel_fn(self.pub_cancel.publish)
self.sub_queue_size = rospy.get_param('actionlib_client_sub_queue_size', -1)
if self.sub_queue_size < 0:
self.sub_queue_size = None
self.status_sub = rospy.Subscriber(rospy.remap_name(ns) + '/status', GoalStatusArray, callback=self._status_cb, queue_size=self.sub_queue_size)
self.result_sub = rospy.Subscriber(rospy.remap_name(ns) + '/result', self.ActionResult, callback=self._result_cb, queue_size=self.sub_queue_size)
self.feedback_sub = rospy.Subscriber(rospy.remap_name(ns) + '/feedback', self.ActionFeedback, callback=self._feedback_cb, queue_size=self.sub_queue_size)
## @brief Stop the action client.
def stop(self):
self.pub_goal.unregister()
self.pub_cancel.unregister()
self.status_sub.unregister()
self.result_sub.unregister()
self.feedback_sub.unregister()
## @brief Sends a goal to the action server
##
## @param goal An instance of the *Goal message.
##
## @param transition_cb Callback that gets called on every client
## state transition for the sent goal. It should take in a
## ClientGoalHandle as an argument.
##
## @param feedback_cb Callback that gets called every time
## feedback is received for the sent goal. It takes two
## parameters: a ClientGoalHandle and an instance of the *Feedback
## message.
##
## @return ClientGoalHandle for the sent goal.
def send_goal(self, goal, transition_cb=None, feedback_cb=None):
return self.manager.init_goal(goal, transition_cb, feedback_cb)
## @brief Cancels all goals currently running on the action server.
##
## Preempts all goals running on the action server at the point
## that the cancel message is serviced by the action server.
def cancel_all_goals(self):
cancel_msg = GoalID(stamp=rospy.Time.from_sec(0.0), id="")
self.pub_cancel.publish(cancel_msg)
## @brief Cancels all goals prior to a given timestamp
##
## This preempts all goals running on the action server for which the
## time stamp is earlier than the specified time stamp
## this message is serviced by the ActionServer.
def cancel_goals_at_and_before_time(self, time):
cancel_msg = GoalID(stamp=time, id="")
self.pub_cancel.publish(cancel_msg)
## @brief [Deprecated] Use wait_for_server
def wait_for_action_server_to_start(self, timeout=rospy.Duration(0.0)):
return self.wait_for_server(timeout)
## @brief Waits for the ActionServer to connect to this client
##
## Often, it can take a second for the action server & client to negotiate
## a connection, thus, risking the first few goals to be dropped. This call lets
## the user wait until the network connection to the server is negotiated
def wait_for_server(self, timeout=rospy.Duration(0.0)):
started = False
timeout_time = rospy.get_rostime() + timeout
while not rospy.is_shutdown():
if self.last_status_msg:
server_id = self.last_status_msg._connection_header['callerid']
if self.pub_goal.impl.has_connection(server_id) and \
self.pub_cancel.impl.has_connection(server_id):
# Check that the connections to the result and feedback
# topics have completed and are ready to receive data.
# Check the connections by checking the callerid from the
# publisher's header against the server_id.
# Note: there is no need to check the status topic, as
# we already received a status message in last_status_msg
# and are using it as the source of truth for the server_id.
# Note: there is no need to grab the c_lock, the
# implementation guarantees to never mutate the connection
# list, only replace the reference. So all we need to do is
# to copy a reference ourselves. This behavior is
# documented in the comment above the c_lock creation in
# rospy._TopicImpl.__init__.
result_sub_connections = self.result_sub.impl.connections
result_sub_found = False
for c in result_sub_connections:
if c.callerid_pub == server_id:
result_sub_found = True
feedback_sub_connections = self.feedback_sub.impl.connections
feedback_sub_found = False
for c in feedback_sub_connections:
if c.callerid_pub == server_id:
feedback_sub_found = True
if result_sub_found and feedback_sub_found:
started = True
break
if timeout != rospy.Duration(0.0) and rospy.get_rostime() >= timeout_time:
break
time.sleep(0.01)
return started
def _status_cb(self, msg):
self.last_status_msg = msg
self.manager.update_statuses(msg)
def _result_cb(self, msg):
self.manager.update_results(msg)
def _feedback_cb(self, msg):
self.manager.update_feedbacks(msg)