Commit ff3753125e043c1203795001fcbc21e82dba533e - ros-actionlib

Imported Upstream version 1.11.6 Jochen Sprickerhof 7 years ago

14 changed file(s) with 699 addition(s) and 718 deletion(s). Raw diff Collapse all Expand all

+23

-0

CHANGELOG.rst less more

0	0	^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
1	1	Changelog for package actionlib
2	2	^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
	3
	4	1.11.6 (2016-06-22)
	5	-------------------
	6	* Python code cleanup (`#43 <https://github.com/ros/actionlib/issues/43>`_)
	7	* Cleaned up semicolons, indentation, spaces.
	8	* Removed unused local var after further confirmation of no risk of side effects.
	9	* Contributors: Andrew Blakey
	10
	11	1.11.5 (2016-03-14)
	12	-------------------
	13	* update maintainer
	14	* Merge pull request `#42 <https://github.com/ros/actionlib/issues/42>`_ from jonbinney/python3-compat
	15	Python 3 compatibility changes
	16	* More readable iteration in state name lookup
	17	* Update syntax for exception handling
	18	* Iterate over dictionary in python3 compatible way
	19	* Use absolute imports for python3 compatibility
	20	* Merge pull request `#39 <https://github.com/ros/actionlib/issues/39>`_ from clearpathrobotics/action-fixup
	21	Minor improvements
	22	* Enable UI feedback for preempt-requested goal in axserver.py
	23	* Clean up axclient.py initialization to allow starting before actionserver, requires action type passed in
	24	* Add hashes to ServerGoalHandle and ClientGoalHandles
	25	* Contributors: Esteve Fernandez, Jon Binney, Mikael Arguedas, Paul Bovbel
3	26
4	27	1.11.4 (2015-04-22)
5	28	-------------------

-2

package.xml less more

0	0	<package>
1	1	<name>actionlib</name>
2		<version>1.11.4</version>
	2	<version>1.11.6</version>
3	3	<description>
4	4	The actionlib stack provides a standardized interface for
5	5	interfacing with preemptable tasks. Examples of this include moving
6	6	the base to a target location, performing a laser scan and returning
7	7	the resulting point cloud, detecting the handle of a door, etc.
8	8	</description>
9		<maintainer email="esteve@osrfoundation.org">Esteve Fernandez</maintainer>
	9	<maintainer email="mikael@osrfoundation.org">Mikael Arguedas</maintainer>
10	10	<license>BSD</license>
11	11
12	12	<url type="website">http://www.ros.org/wiki/actionlib</url>
	13	<url type="bugtracker">https://github.com/ros/actionlib/issues</url>
	14	<url type="repository">https://github.com/ros/actionlib</url>
13	15	<author>Eitan Marder-Eppstein</author>
14	16	<author>Vijay Pradeep</author>
15	17

-10

src/actionlib/__init__.py less more

0	0	# Copyright (c) 2009, Willow Garage, Inc.
1	1	# All rights reserved.
2		#
	2	#
3	3	# Redistribution and use in source and binary forms, with or without
4	4	# modification, are permitted provided that the following conditions are met:
5		#
	5	#
6	6	# * Redistributions of source code must retain the above copyright
7	7	# notice, this list of conditions and the following disclaimer.
8	8	# * Redistributions in binary form must reproduce the above copyright

11	11	# * Neither the name of the Willow Garage, Inc. nor the names of its
12	12	# contributors may be used to endorse or promote products derived from
13	13	# this software without specific prior written permission.
14		#
	14	#
15	15	# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16	16	# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17	17	# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

24	24	# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25	25	# POSSIBILITY OF SUCH DAMAGE.
26	26
27		from action_client import *
28		from simple_action_client import *
29		from action_server import *
30		from simple_action_server import *
31
32
33
	27	from actionlib.action_client import *
	28	from actionlib.simple_action_client import *
	29	from actionlib.action_server import *
	30	from actionlib.simple_action_server import *

+44

-42

src/actionlib/action_client.py less more

59	59
60	60	g_goal_id = 1
61	61
	62
62	63	def get_name_of_constant(C, n):
63		for k,v in C.__dict__.iteritems():
	64	for k, v in C.__dict__.items():
64	65	if type(v) is int and v == n:
65	66	return k
66	67	return "NO_SUCH_STATE_%d" % n

77	78	DONE = 7
78	79	LOST = 8
79	80
	81
80	82	class TerminalState(object):
81	83	RECALLED = GoalStatus.RECALLED
82	84	REJECTED = GoalStatus.REJECTED

90	92	CommState.to_string = classmethod(get_name_of_constant)
91	93	TerminalState.to_string = classmethod(get_name_of_constant)
92	94
	95
93	96	def _find_status_by_goal_id(status_array, id):
94	97	for s in status_array.status_list:
95	98	if s.goal_id.id == id:
96	99	return s
97	100	return None
	101
98	102
99	103	## @brief Client side handle to monitor goal progress.
100	104	##

110	114	def __init__(self, comm_state_machine):
111	115	self.comm_state_machine = comm_state_machine
112	116
113		#print "GH created. id = %.3f" % self.comm_state_machine.action_goal.goal_id.stamp.to_sec()
	117	# print "GH created. id = %.3f" % self.comm_state_machine.action_goal.goal_id.stamp.to_sec()
114	118
115	119	## @brief True iff the two ClientGoalHandle's are tracking the same goal
116	120	def __eq__(self, o):

124	128	return True
125	129	return not (self.comm_state_machine == o.comm_state_machine)
126	130
	131	## @brieft Hash function for ClientGoalHandle
	132	def __hash__(self):
	133	return hash(self.comm_state_machine)
127	134
128	135	## @brief Sends a cancel message for this specific goal to the ActionServer.
129	136	##
130	137	## Also transitions the client state to WAITING_FOR_CANCEL_ACK
131	138	def cancel(self):
132	139	with self.comm_state_machine.mutex:
133		cancel_msg = GoalID(stamp = rospy.Time(0),
134		id = self.comm_state_machine.action_goal.goal_id.id)
	140	cancel_msg = GoalID(stamp=rospy.Time(0),
	141	id=self.comm_state_machine.action_goal.goal_id.id)
135	142	self.comm_state_machine.send_cancel_fn(cancel_msg)
136	143	self.comm_state_machine.transition_to(CommState.WAITING_FOR_CANCEL_ACK)
137	144

196	203	with self.comm_state_machine.mutex:
197	204	if self.comm_state_machine.state != CommState.DONE:
198	205	rospy.logwarn("Asking for the terminal state when we're in [%s]",
199		CommState.to_string(self.comm_state_machine.state))
	206	CommState.to_string(self.comm_state_machine.state))
200	207
201	208	goal_status = self.comm_state_machine.latest_goal_status.status
202	209	if goal_status in [GoalStatus.PREEMPTED, GoalStatus.SUCCEEDED,

206	213
207	214	rospy.logerr("Asking for a terminal state, but the goal status is %d", goal_status)
208	215	return GoalStatus.LOST
209
210
211	216
212	217
213	218	NO_TRANSITION = -1

222	227	GoalStatus.PREEMPTED: (CommState.ACTIVE, CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
223	228	GoalStatus.SUCCEEDED: (CommState.ACTIVE, CommState.WAITING_FOR_RESULT),
224	229	GoalStatus.ABORTED: (CommState.ACTIVE, CommState.WAITING_FOR_RESULT),
225		GoalStatus.PREEMPTING: (CommState.ACTIVE, CommState.PREEMPTING) },
	230	GoalStatus.PREEMPTING: (CommState.ACTIVE, CommState.PREEMPTING)},
226	231	CommState.PENDING: {
227	232	GoalStatus.PENDING: NO_TRANSITION,
228	233	GoalStatus.ACTIVE: CommState.ACTIVE,

232	237	GoalStatus.PREEMPTED: (CommState.ACTIVE, CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
233	238	GoalStatus.SUCCEEDED: (CommState.ACTIVE, CommState.WAITING_FOR_RESULT),
234	239	GoalStatus.ABORTED: (CommState.ACTIVE, CommState.WAITING_FOR_RESULT),
235		GoalStatus.PREEMPTING: (CommState.ACTIVE, CommState.PREEMPTING) },
	240	GoalStatus.PREEMPTING: (CommState.ACTIVE, CommState.PREEMPTING)},
236	241	CommState.ACTIVE: {
237	242	GoalStatus.PENDING: INVALID_TRANSITION,
238	243	GoalStatus.ACTIVE: NO_TRANSITION,

242	247	GoalStatus.PREEMPTED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
243	248	GoalStatus.SUCCEEDED: CommState.WAITING_FOR_RESULT,
244	249	GoalStatus.ABORTED: CommState.WAITING_FOR_RESULT,
245		GoalStatus.PREEMPTING: CommState.PREEMPTING },
	250	GoalStatus.PREEMPTING: CommState.PREEMPTING},
246	251	CommState.WAITING_FOR_RESULT: {
247	252	GoalStatus.PENDING: INVALID_TRANSITION,
248	253	GoalStatus.ACTIVE: NO_TRANSITION,

252	257	GoalStatus.PREEMPTED: NO_TRANSITION,
253	258	GoalStatus.SUCCEEDED: NO_TRANSITION,
254	259	GoalStatus.ABORTED: NO_TRANSITION,
255		GoalStatus.PREEMPTING: INVALID_TRANSITION },
	260	GoalStatus.PREEMPTING: INVALID_TRANSITION},
256	261	CommState.WAITING_FOR_CANCEL_ACK: {
257	262	GoalStatus.PENDING: NO_TRANSITION,
258	263	GoalStatus.ACTIVE: NO_TRANSITION,

262	267	GoalStatus.PREEMPTED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
263	268	GoalStatus.SUCCEEDED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
264	269	GoalStatus.ABORTED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
265		GoalStatus.PREEMPTING: CommState.PREEMPTING },
	270	GoalStatus.PREEMPTING: CommState.PREEMPTING},
266	271	CommState.RECALLING: {
267	272	GoalStatus.PENDING: INVALID_TRANSITION,
268	273	GoalStatus.ACTIVE: INVALID_TRANSITION,

272	277	GoalStatus.PREEMPTED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
273	278	GoalStatus.SUCCEEDED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
274	279	GoalStatus.ABORTED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT),
275		GoalStatus.PREEMPTING: CommState.PREEMPTING },
	280	GoalStatus.PREEMPTING: CommState.PREEMPTING},
276	281	CommState.PREEMPTING: {
277	282	GoalStatus.PENDING: INVALID_TRANSITION,
278	283	GoalStatus.ACTIVE: INVALID_TRANSITION,

282	287	GoalStatus.PREEMPTED: CommState.WAITING_FOR_RESULT,
283	288	GoalStatus.SUCCEEDED: CommState.WAITING_FOR_RESULT,
284	289	GoalStatus.ABORTED: CommState.WAITING_FOR_RESULT,
285		GoalStatus.PREEMPTING: NO_TRANSITION },
	290	GoalStatus.PREEMPTING: NO_TRANSITION},
286	291	CommState.DONE: {
287	292	GoalStatus.PENDING: INVALID_TRANSITION,
288	293	GoalStatus.ACTIVE: INVALID_TRANSITION,

292	297	GoalStatus.PREEMPTED: NO_TRANSITION,
293	298	GoalStatus.SUCCEEDED: NO_TRANSITION,
294	299	GoalStatus.ABORTED: NO_TRANSITION,
295		GoalStatus.PREEMPTING: INVALID_TRANSITION } }
296
	300	GoalStatus.PREEMPTING: INVALID_TRANSITION}}
297	301
298	302
299	303	class CommStateMachine:

306	310
307	311	self.state = CommState.WAITING_FOR_GOAL_ACK
308	312	self.mutex = threading.RLock()
309		self.latest_goal_status = GoalStatus(status = GoalStatus.PENDING)
	313	self.latest_goal_status = GoalStatus(status=GoalStatus.PENDING)
310	314	self.latest_result = None
311	315
312	316	def __eq__(self, o):
313	317	return self.action_goal.goal_id.id == o.action_goal.goal_id.id
	318
	319	## @brieft Hash function for CommStateMachine
	320	def __hash__(self):
	321	return hash(self.action_goal.goal_id.id)
314	322
315	323	def set_state(self, state):
316	324	rospy.logdebug("Transitioning CommState from %s to %s",

403	411	if self.action_goal.goal_id.id != action_feedback.status.goal_id.id:
404	412	return
405	413
406		#with self.mutex:
	414	# with self.mutex:
407	415	if self.feedback_cb and self.state != CommState.DONE:
408	416	self.feedback_cb(ClientGoalHandle(self), action_feedback.feedback)
409	417

431	439	global g_goal_id
432	440	id, g_goal_id = g_goal_id, g_goal_id + 1
433	441	now = rospy.Time.now()
434		return GoalID(id = "%s-%i-%.3f" % \
435		(rospy.get_caller_id(), id, now.to_sec()), stamp = now)
	442	return GoalID(id="%s-%i-%.3f" % (rospy.get_caller_id(), id, now.to_sec()), stamp=now)
436	443
437	444	def register_send_goal_fn(self, fn):
438	445	self.send_goal_fn = fn
	446
439	447	def register_cancel_fn(self, fn):
440	448	self.cancel_fn = fn
441	449
442	450	## Sends off a goal and starts tracking its status.
443	451	##
444	452	## @return ClientGoalHandle for the sent goal.
445		def init_goal(self, goal, transition_cb = None, feedback_cb = None):
446		action_goal = self.ActionGoal(header = Header(),
447		goal_id = self._generate_id(),
448		goal = goal)
	453	def init_goal(self, goal, transition_cb=None, feedback_cb=None):
	454	action_goal = self.ActionGoal(header=Header(),
	455	goal_id=self._generate_id(),
	456	goal=goal)
449	457	action_goal.header.stamp = rospy.get_rostime()
450	458
451	459	csm = CommStateMachine(action_goal, transition_cb, feedback_cb,

457	465	self.send_goal_fn(action_goal)
458	466
459	467	return ClientGoalHandle(csm)
460
461	468
462	469	# Pulls out the statuses that are still live (creating strong
463	470	# references to them)

467	474	live_statuses = filter(lambda x: x, live_statuses)
468	475	return live_statuses
469	476
470
471	477	## Updates the statuses of all goals from the information in status_array.
472	478	##
473	479	## @param status_array (\c actionlib_msgs/GoalStatusArray)
474	480	def update_statuses(self, status_array):
475		live_statuses = []
476	481
477	482	with self.list_mutex:
478	483	# Garbage collects dead status objects

481	486	for status in self._get_live_statuses():
482	487	status.update_status(status_array)
483	488
484
485	489	def update_results(self, action_result):
486	490	for status in self._get_live_statuses():
487	491	status.update_result(action_result)

489	493	def update_feedbacks(self, action_feedback):
490	494	for status in self._get_live_statuses():
491	495	status.update_feedback(action_feedback)
	496
492	497
493	498	class ActionClient:
494	499	## @brief Constructs an ActionClient and opens connections to an ActionServer.

537	542	## message.
538	543	##
539	544	## @return ClientGoalHandle for the sent goal.
540		def send_goal(self, goal, transition_cb = None, feedback_cb = None):
	545	def send_goal(self, goal, transition_cb=None, feedback_cb=None):
541	546	return self.manager.init_goal(goal, transition_cb, feedback_cb)
542	547
543	548	## @brief Cancels all goals currently running on the action server.

545	550	## Preempts all goals running on the action server at the point
546	551	## that the cancel message is serviced by the action server.
547	552	def cancel_all_goals(self):
548		cancel_msg = GoalID(stamp = rospy.Time.from_sec(0.0),
549		id = "")
	553	cancel_msg = GoalID(stamp=rospy.Time.from_sec(0.0), id="")
550	554	self.pub_cancel.publish(cancel_msg)
551	555
552	556	## @brief Cancels all goals prior to a given timestamp
553	557	##
554		## This preempts all goals running on the action server for which the
	558	## This preempts all goals running on the action server for which the
555	559	## time stamp is earlier than the specified time stamp
556	560	## this message is serviced by the ActionServer.
557	561
558	562	def cancel_goals_at_and_before_time(self, time):
559		cancel_msg = GoalID(stamp = time, id = "")
	563	cancel_msg = GoalID(stamp=time, id="")
560	564	self.pub_cancel.publish(cancel_msg)
561
562
	565
	566
563	567	## @brief [Deprecated] Use wait_for_server
564		def wait_for_action_server_to_start(self, timeout = rospy.Duration(0.0)):
	568	def wait_for_action_server_to_start(self, timeout=rospy.Duration(0.0)):
565	569	return self.wait_for_server(timeout)
566	570
567	571	## @brief Waits for the ActionServer to connect to this client

569	573	## Often, it can take a second for the action server & client to negotiate
570	574	## a connection, thus, risking the first few goals to be dropped. This call lets
571	575	## the user wait until the network connection to the server is negotiated
572		def wait_for_server(self, timeout = rospy.Duration(0.0)):
	576	def wait_for_server(self, timeout=rospy.Duration(0.0)):
573	577	started = False
574	578	timeout_time = rospy.get_rostime() + timeout
575	579	while not rospy.is_shutdown():

578	582
579	583	if self.pub_goal.impl.has_connection(server_id) and \
580	584	self.pub_cancel.impl.has_connection(server_id):
581		#We'll also check that all of the subscribers have at least
582		#one publisher, this isn't a perfect check, but without
583		#publisher callbacks... it'll have to do
	585	# We'll also check that all of the subscribers have at least
	586	# one publisher, this isn't a perfect check, but without
	587	# publisher callbacks... it'll have to do
584	588	status_num_pubs = 0
585	589	for stat in self.status_sub.impl.get_stats()[1]:
586	590	if stat[4]:

616	620
617	621	def _feedback_cb(self, msg):
618	622	self.manager.update_feedbacks(msg)
619
620

+189

-199

src/actionlib/action_server.py less more

27	27	#
28	28	# Author: Alexander Sorokin.
29	29	# Based on C++ action_server.h by Eitan Marder-Eppstein
30
31	30	import rospy
32
33	31	import threading
34	32
35	33	from actionlib_msgs.msg import *
36	34
37		from goal_id_generator import GoalIDGenerator
38		from status_tracker import StatusTracker
39
40		from handle_tracker_deleter import HandleTrackerDeleter
41		from server_goal_handle import ServerGoalHandle
	35	from actionlib.goal_id_generator import GoalIDGenerator
	36	from actionlib.status_tracker import StatusTracker
	37
	38	from actionlib.handle_tracker_deleter import HandleTrackerDeleter
	39	from actionlib.server_goal_handle import ServerGoalHandle
42	40
43	41	from actionlib.exceptions import *
44	42

46	44	def nop_cb(goal_handle):
47	45	pass
48	46
49		def ros_timer(callable,frequency):
	47
	48	def ros_timer(callable, frequency):
50	49	rate = rospy.Rate(frequency)
51	50
52		rospy.logdebug("Starting timer");
	51	rospy.logdebug("Starting timer")
53	52	while not rospy.is_shutdown():
54	53	try:
55	54	rate.sleep()
56	55	callable()
57	56	except rospy.exceptions.ROSInterruptException:
58		rospy.logdebug("Sleep interrupted");
	57	rospy.logdebug("Sleep interrupted")
	58
59	59
60	60	## @class ActionServer
61	61	## @brief The ActionServer is a helpful tool for managing goal requests to a

72	72	## @param goal_cb A goal callback to be called when the ActionServer receives a new goal over the wire
73	73	## @param cancel_cb A cancel callback to be called when the ActionServer receives a new cancel request over the wire
74	74	## @param auto_start A boolean value that tells the ActionServer wheteher or not to start publishing as soon as it comes up. THIS SHOULD ALWAYS BE SET TO FALSE TO AVOID RACE CONDITIONS and start() should be called after construction of the server.
75		def __init__(self, ns, ActionSpec, goal_cb, cancel_cb = nop_cb, auto_start = True):
76		self.ns=ns;
	75	def __init__(self, ns, ActionSpec, goal_cb, cancel_cb=nop_cb, auto_start=True):
	76	self.ns = ns
77	77
78	78	try:
79	79	a = ActionSpec()

86	86	except AttributeError:
87	87	raise ActionException("Type is not an action spec: %s" % str(ActionSpec))
88	88
89
90	89	self.goal_sub = None
91	90	self.cancel_sub = None
92	91	self.status_pub = None

95	94
96	95	self.lock = threading.RLock()
97	96
98		self.status_timer = None;
99
100		self.status_list = [];
101
102		self.last_cancel = rospy.Time();
103		self.status_list_timeout = rospy.Duration();
104
105		self.id_generator = GoalIDGenerator();
106
107		self.goal_callback = goal_cb;
108		assert(self.goal_callback);
109
110		self.cancel_callback = cancel_cb;
111		self.auto_start = auto_start;
112
113		self.started = False;
	97	self.status_timer = None
	98
	99	self.status_list = []
	100
	101	self.last_cancel = rospy.Time()
	102	self.status_list_timeout = rospy.Duration()
	103
	104	self.id_generator = GoalIDGenerator()
	105
	106	self.goal_callback = goal_cb
	107	assert(self.goal_callback)
	108
	109	self.cancel_callback = cancel_cb
	110	self.auto_start = auto_start
	111
	112	self.started = False
114	113
115	114	if self.auto_start:
116		rospy.logwarn("You've passed in true for auto_start to the python action server, you should always pass in false to avoid race conditions.")
	115	rospy.logwarn("You've passed in true for auto_start to the python action server, you should always pass "
	116	"in false to avoid race conditions.")
117	117	self.start()
118	118
119	119

124	124
125	125	## @brief Register a callback to be invoked when a new cancel is received, this will replace any previously registered callback
126	126	## @param cb The callback to invoke
127		def register_cancel_callback(self,cancel_cb):
128		self.cancel_callback = cancel_cb
	127	def register_cancel_callback(self, cancel_cb):
	128	self.cancel_callback = cancel_cb
129	129
130	130	## @brief Start the action server
131	131	def start(self):
132	132	with self.lock:
133		self.initialize();
134		self.started = True;
135		self.publish_status();
	133	self.initialize()
	134	self.started = True
	135	self.publish_status()
136	136
137	137	## @brief Initialize all ROS connections and setup timers
138	138	def initialize(self):
139		self.status_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/status", GoalStatusArray, latch=True, queue_size=50);
140		self.result_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/result", self.ActionResult, queue_size=50);
141		self.feedback_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/feedback", self.ActionFeedback, queue_size=50);
142
143		self.goal_sub = rospy.Subscriber(rospy.remap_name(self.ns)+"/goal", self.ActionGoal,self.internal_goal_callback);
144
145		self.cancel_sub = rospy.Subscriber(rospy.remap_name(self.ns)+"/cancel", GoalID,self.internal_cancel_callback);
146
147		#read the frequency with which to publish status from the parameter server
148		#if not specified locally explicitly, use search param to find actionlib_status_frequency
149		resolved_status_frequency_name = rospy.remap_name(self.ns)+"/status_frequency"
150		if rospy.has_param(resolved_status_frequency_name):
151		self.status_frequency = rospy.get_param(resolved_status_frequency_name, 5.0);
152		rospy.logwarn("You're using the deprecated status_frequency parameter, please switch to actionlib_status_frequency.")
153		else:
154		search_status_frequency_name = rospy.search_param("actionlib_status_frequency")
155		if search_status_frequency_name is None:
156		self.status_frequency = 5.0
157		else:
158		self.status_frequency = rospy.get_param(search_status_frequency_name, 5.0)
159
160		status_list_timeout = rospy.get_param(rospy.remap_name(self.ns)+"/status_list_timeout", 5.0);
161		self.status_list_timeout = rospy.Duration(status_list_timeout);
162
163
164		self.status_timer = threading.Thread( None, ros_timer, None, (self.publish_status_async,self.status_frequency) );
165		self.status_timer.start();
	139	self.status_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/status", GoalStatusArray, latch=True, queue_size=50)
	140	self.result_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/result", self.ActionResult, queue_size=50)
	141	self.feedback_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/feedback", self.ActionFeedback, queue_size=50)
	142
	143	self.goal_sub = rospy.Subscriber(rospy.remap_name(self.ns)+"/goal", self.ActionGoal, self.internal_goal_callback)
	144
	145	self.cancel_sub = rospy.Subscriber(rospy.remap_name(self.ns)+"/cancel", GoalID, self.internal_cancel_callback)
	146
	147	# read the frequency with which to publish status from the parameter server
	148	# if not specified locally explicitly, use search param to find actionlib_status_frequency
	149	resolved_status_frequency_name = rospy.remap_name(self.ns)+"/status_frequency"
	150	if rospy.has_param(resolved_status_frequency_name):
	151	self.status_frequency = rospy.get_param(resolved_status_frequency_name, 5.0)
	152	rospy.logwarn("You're using the deprecated status_frequency parameter, please switch to actionlib_status_frequency.")
	153	else:
	154	search_status_frequency_name = rospy.search_param("actionlib_status_frequency")
	155	if search_status_frequency_name is None:
	156	self.status_frequency = 5.0
	157	else:
	158	self.status_frequency = rospy.get_param(search_status_frequency_name, 5.0)
	159
	160	status_list_timeout = rospy.get_param(rospy.remap_name(self.ns)+"/status_list_timeout", 5.0)
	161	self.status_list_timeout = rospy.Duration(status_list_timeout)
	162
	163	self.status_timer = threading.Thread(None, ros_timer, None, (self.publish_status_async, self.status_frequency))
	164	self.status_timer.start()
166	165
167	166	## @brief Publishes a result for a given goal
168	167	## @param status The status of the goal with which the result is associated
169	168	## @param result The result to publish
170	169	def publish_result(self, status, result):
171	170	with self.lock:
172		ar = self.ActionResult();
173		ar.header.stamp = rospy.Time.now();
174		ar.status = status;
175		ar.result = result;
176		self.result_pub.publish(ar);
	171	ar = self.ActionResult()
	172	ar.header.stamp = rospy.Time.now()
	173	ar.status = status
	174	ar.result = result
	175	self.result_pub.publish(ar)
177	176	self.publish_status()
178	177
179	178

181	180	## @param status The status of the goal with which the feedback is associated
182	181	## @param feedback The feedback to publish
183	182	def publish_feedback(self, status, feedback):
184		with self.lock:
185
186		af=self.ActionFeedback();
187		af.header.stamp = rospy.Time.now();
188		af.status = status;
189		af.feedback = feedback;
190		self.feedback_pub.publish(af);
	183	with self.lock:
	184	af = self.ActionFeedback()
	185	af.header.stamp = rospy.Time.now()
	186	af.status = status
	187	af.feedback = feedback
	188	self.feedback_pub.publish(af)
191	189
192	190
193	191	## @brief The ROS callback for cancel requests coming into the ActionServer
194	192	def internal_cancel_callback(self, goal_id):
195		with self.lock:
196
197		#if we're not started... then we're not actually going to do anything
198		if not self.started:
199		return;
200
201		#we need to handle a cancel for the user
202		rospy.logdebug("The action server has received a new cancel request");
203
204		goal_id_found = False;
205		for st in self.status_list[:]:
206		#check if the goal id is zero or if it is equal to the goal id of
207		#the iterator or if the time of the iterator warrants a cancel
208
209		cancel_everything = (goal_id.id == "" and goal_id.stamp == rospy.Time() ) #rospy::Time()) #id and stamp 0 --> cancel everything
210		cancel_this_one = ( goal_id.id == st.status.goal_id.id) #ids match... cancel that goal
211		cancel_before_stamp = (goal_id.stamp != rospy.Time() and st.status.goal_id.stamp <= goal_id.stamp) #//stamp != 0 --> cancel everything before stamp
212
213		if cancel_everything or cancel_this_one or cancel_before_stamp:
214		#we need to check if we need to store this cancel request for later
215		if goal_id.id == st.status.goal_id.id:
216		goal_id_found = True;
217
218		#attempt to get the handle_tracker for the list item if it exists
219		handle_tracker = st.handle_tracker;
220
221		if handle_tracker is None:
222		#if the handle tracker is expired, then we need to create a new one
223		handle_tracker = HandleTrackerDeleter(self, st);
224		st.handle_tracker = handle_tracker;
225
226		#we also need to reset the time that the status is supposed to be removed from the list
227		st.handle_destruction_time = rospy.Time.now()
228
229
230		#set the status of the goal to PREEMPTING or RECALLING as approriate
231		#and check if the request should be passed on to the user
232		gh = ServerGoalHandle(st, self, handle_tracker);
233		if gh.set_cancel_requested():
234		#call the user's cancel callback on the relevant goal
235		self.cancel_callback(gh);
236
237
238		#if the requested goal_id was not found, and it is non-zero, then we need to store the cancel request
239		if goal_id.id != "" and not goal_id_found:
240		tracker= StatusTracker(goal_id, actionlib_msgs.msg.GoalStatus.RECALLING);
241		self.status_list.append(tracker)
242		#start the timer for how long the status will live in the list without a goal handle to it
243		tracker.handle_destruction_time = rospy.Time.now()
244
245		#make sure to set last_cancel_ based on the stamp associated with this cancel request
246		if goal_id.stamp > self.last_cancel:
247		self.last_cancel = goal_id.stamp;
	193	with self.lock:
	194
	195	# if we're not started... then we're not actually going to do anything
	196	if not self.started:
	197	return
	198
	199	# we need to handle a cancel for the user
	200	rospy.logdebug("The action server has received a new cancel request")
	201
	202	goal_id_found = False
	203	for st in self.status_list[:]:
	204	# check if the goal id is zero or if it is equal to the goal id of
	205	# the iterator or if the time of the iterator warrants a cancel
	206
	207	cancel_everything = (goal_id.id == "" and goal_id.stamp == rospy.Time()) # rospy::Time()) #id and stamp 0 --> cancel everything
	208	cancel_this_one = (goal_id.id == st.status.goal_id.id) # ids match... cancel that goal
	209	cancel_before_stamp = (goal_id.stamp != rospy.Time() and st.status.goal_id.stamp <= goal_id.stamp) # //stamp != 0 --> cancel everything before stamp
	210
	211	if cancel_everything or cancel_this_one or cancel_before_stamp:
	212	# we need to check if we need to store this cancel request for later
	213	if goal_id.id == st.status.goal_id.id:
	214	goal_id_found = True
	215
	216	# attempt to get the handle_tracker for the list item if it exists
	217	handle_tracker = st.handle_tracker
	218
	219	if handle_tracker is None:
	220	# if the handle tracker is expired, then we need to create a new one
	221	handle_tracker = HandleTrackerDeleter(self, st)
	222	st.handle_tracker = handle_tracker
	223
	224	# we also need to reset the time that the status is supposed to be removed from the list
	225	st.handle_destruction_time = rospy.Time.now()
	226
	227	# set the status of the goal to PREEMPTING or RECALLING as approriate
	228	# and check if the request should be passed on to the user
	229	gh = ServerGoalHandle(st, self, handle_tracker)
	230	if gh.set_cancel_requested():
	231	# call the user's cancel callback on the relevant goal
	232	self.cancel_callback(gh)
	233
	234	# if the requested goal_id was not found, and it is non-zero, then we need to store the cancel request
	235	if goal_id.id != "" and not goal_id_found:
	236	tracker = StatusTracker(goal_id, actionlib_msgs.msg.GoalStatus.RECALLING)
	237	self.status_list.append(tracker)
	238	# start the timer for how long the status will live in the list without a goal handle to it
	239	tracker.handle_destruction_time = rospy.Time.now()
	240
	241	# make sure to set last_cancel_ based on the stamp associated with this cancel request
	242	if goal_id.stamp > self.last_cancel:
	243	self.last_cancel = goal_id.stamp
248	244
249	245
250	246	## @brief The ROS callback for goals coming into the ActionServer
251	247	def internal_goal_callback(self, goal):
252		with self.lock:
253		#if we're not started... then we're not actually going to do anything
254		if not self.started:
255		return;
256
257		rospy.logdebug("The action server has received a new goal request");
258
259		#we need to check if this goal already lives in the status list
260		for st in self.status_list[:]:
261		if goal.goal_id.id == st.status.goal_id.id:
262		rospy.logdebug("Goal %s was already in the status list with status %i" % (goal.goal_id.id, st.status.status))
263		# Goal could already be in recalling state if a cancel came in before the goal
264		if st.status.status == actionlib_msgs.msg.GoalStatus.RECALLING:
265		st.status.status = actionlib_msgs.msg.GoalStatus.RECALLED
266		self.publish_result(st.status, self.ActionResultType())
267
268		#if this is a request for a goal that has no active handles left,
269		#we'll bump how long it stays in the list
270		if st.handle_tracker is None:
271		st.handle_destruction_time = rospy.Time.now()
272
273		#make sure not to call any user callbacks or add duplicate status onto the list
274		return;
275
276		#if the goal is not in our list, we need to create a StatusTracker associated with this goal and push it on
277		st = StatusTracker(None,None,goal)
278		self.status_list.append(st);
279
280		#we need to create a handle tracker for the incoming goal and update the StatusTracker
281		handle_tracker = HandleTrackerDeleter(self, st);
282
283		st.handle_tracker = handle_tracker;
284
285		#check if this goal has already been canceled based on its timestamp
286		gh= ServerGoalHandle(st, self, handle_tracker);
287		if goal.goal_id.stamp != rospy.Time() and goal.goal_id.stamp <= self.last_cancel:
288		#if it has... just create a GoalHandle for it and setCanceled
289		gh.set_canceled(None, "This goal handle was canceled by the action server because its timestamp is before the timestamp of the last cancel request");
290		else:
291		#now, we need to create a goal handle and call the user's callback
292		self.goal_callback(gh);
293
	248	with self.lock:
	249	# if we're not started... then we're not actually going to do anything
	250	if not self.started:
	251	return
	252
	253	rospy.logdebug("The action server has received a new goal request")
	254
	255	# we need to check if this goal already lives in the status list
	256	for st in self.status_list[:]:
	257	if goal.goal_id.id == st.status.goal_id.id:
	258	rospy.logdebug("Goal %s was already in the status list with status %i" % (goal.goal_id.id, st.status.status))
	259	# Goal could already be in recalling state if a cancel came in before the goal
	260	if st.status.status == actionlib_msgs.msg.GoalStatus.RECALLING:
	261	st.status.status = actionlib_msgs.msg.GoalStatus.RECALLED
	262	self.publish_result(st.status, self.ActionResultType())
	263
	264	# if this is a request for a goal that has no active handles left,
	265	# we'll bump how long it stays in the list
	266	if st.handle_tracker is None:
	267	st.handle_destruction_time = rospy.Time.now()
	268
	269	# make sure not to call any user callbacks or add duplicate status onto the list
	270	return
	271
	272	# if the goal is not in our list, we need to create a StatusTracker associated with this goal and push it on
	273	st = StatusTracker(None, None, goal)
	274	self.status_list.append(st)
	275
	276	# we need to create a handle tracker for the incoming goal and update the StatusTracker
	277	handle_tracker = HandleTrackerDeleter(self, st)
	278
	279	st.handle_tracker = handle_tracker
	280
	281	# check if this goal has already been canceled based on its timestamp
	282	gh = ServerGoalHandle(st, self, handle_tracker)
	283	if goal.goal_id.stamp != rospy.Time() and goal.goal_id.stamp <= self.last_cancel:
	284	# if it has... just create a GoalHandle for it and setCanceled
	285	gh.set_canceled(None, "This goal handle was canceled by the action server because its timestamp is before the timestamp of the last cancel request")
	286	else:
	287	# now, we need to create a goal handle and call the user's callback
	288	self.goal_callback(gh)
294	289
295	290	## @brief Publish status for all goals on a timer event
296	291	def publish_status_async(self):
297		rospy.logdebug("Status async");
298		with self.lock:
299		#we won't publish status unless we've been started
300		if not self.started:
301		return
302		self.publish_status();
303
304
	292	rospy.logdebug("Status async")
	293	with self.lock:
	294	# we won't publish status unless we've been started
	295	if not self.started:
	296	return
	297	self.publish_status()
305	298
306	299	## @brief Explicitly publish status
307	300	def publish_status(self):
308		with self.lock:
309		#build a status array
310		status_array = actionlib_msgs.msg.GoalStatusArray()
311
312		#status_array.set_status_list_size(len(self.status_list));
313
314		i=0;
315		while i<len(self.status_list):
316		st = self.status_list[i]
317		#check if the item is due for deletion from the status list
318		if st.handle_destruction_time != rospy.Time() and st.handle_destruction_time + self.status_list_timeout < rospy.Time.now():
319		rospy.logdebug("Item %s with destruction time of %.3f being removed from list. Now = %.3f" % \
	301	with self.lock:
	302	# build a status array
	303	status_array = actionlib_msgs.msg.GoalStatusArray()
	304
	305	# status_array.set_status_list_size(len(self.status_list));
	306
	307	i = 0
	308	while i < len(self.status_list):
	309	st = self.status_list[i]
	310	# check if the item is due for deletion from the status list
	311	if st.handle_destruction_time != rospy.Time() and st.handle_destruction_time + self.status_list_timeout < rospy.Time.now():
	312	rospy.logdebug("Item %s with destruction time of %.3f being removed from list. Now = %.3f" %
320	313	(st.status.goal_id, st.handle_destruction_time.to_sec(), rospy.Time.now().to_sec()))
321		del self.status_list[i]
322		else:
323		status_array.status_list.append(st.status);
324		i+=1
325
326
327		status_array.header.stamp = rospy.Time.now()
328		self.status_pub.publish(status_array)
329
330
	314	del self.status_list[i]
	315	else:
	316	status_array.status_list.append(st.status)
	317	i += 1
	318
	319	status_array.header.stamp = rospy.Time.now()
	320	self.status_pub.publish(status_array)

-1

src/actionlib/exceptions.py less more

27	27
28	28	# Author: Jonathan Bohren
29	29
30		class ActionException(Exception): pass
	30
	31	class ActionException(Exception):
	32	pass

+17

-24

src/actionlib/goal_id_generator.py less more

0	0	#! /usr/bin/env python
1	1	# Copyright (c) 2009, Willow Garage, Inc.
2	2	# All rights reserved.
3		#
	3	#
4	4	# Redistribution and use in source and binary forms, with or without
5	5	# modification, are permitted provided that the following conditions are met:
6		#
	6	#
7	7	# * Redistributions of source code must retain the above copyright
8	8	# notice, this list of conditions and the following disclaimer.
9	9	# * Redistributions in binary form must reproduce the above copyright

12	12	# * Neither the name of the Willow Garage, Inc. nor the names of its
13	13	# contributors may be used to endorse or promote products derived from
14	14	# this software without specific prior written permission.
15		#
	15	#
16	16	# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
17	17	# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18	18	# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

25	25	# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
26	26	# POSSIBILITY OF SUCH DAMAGE.
27	27
28		# Author: Alexander Sorokin.
	28	# Author: Alexander Sorokin.
29	29	# Based on C++ goal_id_generator.h/cpp
30
31	30	import rospy
32
33		import sys
34	31
35	32	from actionlib_msgs.msg import GoalID
36	33	import threading
37	34
38	35	global s_goalcount_lock
39	36	global s_goalcount
40		s_goalcount_lock = threading.Lock();
	37	s_goalcount_lock = threading.Lock()
41	38	s_goalcount = 0
42	39
43	40
44	41	class GoalIDGenerator:
45	42
46
47		def __init__(self,name=None):
	43	def __init__(self, name=None):
48	44	"""
49	45	* Create a generator that prepends the fully qualified node name to the Goal ID
50	46	* \param name Unique name to prepend to the goal id. This will

53	49	if name is not None:
54	50	self.set_name(name)
55	51	else:
56		self.set_name(rospy.get_name());
	52	self.set_name(rospy.get_name())
57	53
58
59		def set_name(self,name):
	54	def set_name(self, name):
60	55	"""
61	56	* \param name Set the name to prepend to the goal id. This will
62	57	* generally be a fully qualified node name.
63	58	"""
64		self.name=name;
65
66
	59	self.name = name
67	60
68	61	def generate_ID(self):
69	62	"""
70	63	* \brief Generates a unique ID
71	64	* \return A unique GoalID for this action
72	65	"""
73		id = GoalID();
74		cur_time = rospy.Time.now();
75		ss = self.name + "-";
	66	id = GoalID()
	67	cur_time = rospy.Time.now()
	68	ss = self.name + "-"
76	69	global s_goalcount_lock
77	70	global s_goalcount
78	71	with s_goalcount_lock:
79	72	s_goalcount += 1
80		ss += str(s_goalcount) + "-";
81		ss += str(cur_time.secs) + "." + str(cur_time.nsecs);
	73	ss += str(s_goalcount) + "-"
	74	ss += str(cur_time.secs) + "." + str(cur_time.nsecs)
82	75
83		id.id = ss;
84		id.stamp = cur_time;
85		return id;
	76	id.id = ss
	77	id.stamp = cur_time
	78	return id

-9

src/actionlib/handle_tracker_deleter.py less more

0	0	# Copyright (c) 2009, Willow Garage, Inc.
1	1	# All rights reserved.
2		#
	2	#
3	3	# Redistribution and use in source and binary forms, with or without
4	4	# modification, are permitted provided that the following conditions are met:
5		#
	5	#
6	6	# * Redistributions of source code must retain the above copyright
7	7	# notice, this list of conditions and the following disclaimer.
8	8	# * Redistributions in binary form must reproduce the above copyright

11	11	# * Neither the name of the Willow Garage, Inc. nor the names of its
12	12	# contributors may be used to endorse or promote products derived from
13	13	# this software without specific prior written permission.
14		#
	14	#
15	15	# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16	16	# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17	17	# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

24	24	# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25	25	# POSSIBILITY OF SUCH DAMAGE.
26	26
27		# Author: Alexander Sorokin.
	27	# Author: Alexander Sorokin.
28	28	# Based on C++ goal_id_generator.h/cpp
	29	import rospy
29	30
30		import rospy
31	31
32	32	class HandleTrackerDeleter:
33	33	"""

41	41	"""
42	42	@brief create deleter
43	43	"""
44		self.action_server = action_server;
45		self.status_tracker = status_tracker;
	44	self.action_server = action_server
	45	self.status_tracker = status_tracker
46	46
47		def __call__(self,ptr):
	47	def __call__(self, ptr):
48	48	if self.action_server:
49	49	with self.action_server.lock:
50		self.status_tracker.handle_destruction_time = rospy.Time.now();
	50	self.status_tracker.handle_destruction_time = rospy.Time.now()

+173

-188

src/actionlib/server_goal_handle.py less more

0	0	# Copyright (c) 2009, Willow Garage, Inc.
1	1	# All rights reserved.
2		#
	2	#
3	3	# Redistribution and use in source and binary forms, with or without
4	4	# modification, are permitted provided that the following conditions are met:
5		#
	5	#
6	6	# * Redistributions of source code must retain the above copyright
7	7	# notice, this list of conditions and the following disclaimer.
8	8	# * Redistributions in binary form must reproduce the above copyright

11	11	# * Neither the name of the Willow Garage, Inc. nor the names of its
12	12	# contributors may be used to endorse or promote products derived from
13	13	# this software without specific prior written permission.
14		#
	14	#
15	15	# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16	16	# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17	17	# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

24	24	# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25	25	# POSSIBILITY OF SUCH DAMAGE.
26	26
27		# Author: Alexander Sorokin.
	27	# Author: Alexander Sorokin.
28	28	# Based on C++ goal_id_generator.h/cpp
29
30	29	import rospy
31	30
32	31	import actionlib_msgs.msg
	32
33	33
34	34	class ServerGoalHandle:
35	35	"""

45	45	A private constructor used by the ActionServer to initialize a ServerGoalHandle.
46	46	@node The default constructor was not ported.
47	47	"""
48		self.status_tracker = status_tracker;
	48	self.status_tracker = status_tracker
49	49	self.action_server = action_server
50		self.handle_tracker = handle_tracker;
	50	self.handle_tracker = handle_tracker
51	51
52	52	if status_tracker:
53		self.goal=status_tracker.goal;
54		else:
55		self.goal=None
56
	53	self.goal = status_tracker.goal
	54	else:
	55	self.goal = None
57	56
58	57	def get_default_result(self):
59		return self.action_server.ActionResultType();
	58	return self.action_server.ActionResultType()
60	59
61	60	def set_accepted(self, text=""):
62		"""
63		Accept the goal referenced by the goal handle. This will
64		transition to the ACTIVE state or the PREEMPTING state depending
65		on whether a cancel request has been received for the goal
66		"""
67
68		rospy.logdebug("Accepting goal, id: %s, stamp: %.2f", self.get_goal_id().id, self.get_goal_id().stamp.to_sec());
69		if self.goal:
70		with self.action_server.lock:
71		status = self.status_tracker.status.status;
72
73		#if we were pending before, then we'll go active
74		if(status == actionlib_msgs.msg.GoalStatus.PENDING):
75		self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.ACTIVE;
76		self.status_tracker.status.text = text
77		self.action_server.publish_status();
78
79		#if we were recalling before, now we'll go to preempting
80		elif(status == actionlib_msgs.msg.GoalStatus.RECALLING) :
81		self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.PREEMPTING;
82		self.status_tracker.status.text = text
83		self.action_server.publish_status();
84
85		else:
86		rospy.logerr("To transition to an active state, the goal must be in a pending or recalling state, it is currently in state: %d", self.status_tracker.status.status);
87
88		else:
89		rospy.logerr("Attempt to set status on an uninitialized ServerGoalHandle");
90
	61	"""
	62	Accept the goal referenced by the goal handle. This will
	63	transition to the ACTIVE state or the PREEMPTING state depending
	64	on whether a cancel request has been received for the goal
	65	"""
	66
	67	rospy.logdebug("Accepting goal, id: %s, stamp: %.2f", self.get_goal_id().id, self.get_goal_id().stamp.to_sec());
	68	if self.goal:
	69	with self.action_server.lock:
	70	status = self.status_tracker.status.status
	71
	72	# if we were pending before, then we'll go active
	73	if status == actionlib_msgs.msg.GoalStatus.PENDING:
	74	self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.ACTIVE
	75	self.status_tracker.status.text = text
	76	self.action_server.publish_status()
	77
	78	# if we were recalling before, now we'll go to preempting
	79	elif status == actionlib_msgs.msg.GoalStatus.RECALLING:
	80	self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.PREEMPTING
	81	self.status_tracker.status.text = text
	82	self.action_server.publish_status()
	83
	84	else:
	85	rospy.logerr("To transition to an active state, the goal must be in a pending or recalling state, it is currently in state: %d", self.status_tracker.status.status)
	86
	87	else:
	88	rospy.logerr("Attempt to set status on an uninitialized ServerGoalHandle")
91	89
92	90	def set_canceled(self, result=None, text=""):
93	91	"""

96	94	@param result Optionally, the user can pass in a result to be sent to any clients of the goal
97	95	"""
98	96	if not result:
99		result=self.get_default_result();
100
101		rospy.logdebug("Setting status to canceled on goal, id: %s, stamp: %.2f", self.get_goal_id().id, self.get_goal_id().stamp.to_sec());
102
103		if(self.goal):
	97	result = self.get_default_result()
	98
	99	rospy.logdebug("Setting status to canceled on goal, id: %s, stamp: %.2f", self.get_goal_id().id, self.get_goal_id().stamp.to_sec())
	100
	101	if self.goal:
104	102	with self.action_server.lock:
105	103	status = self.status_tracker.status.status;
106		if(status == actionlib_msgs.msg.GoalStatus.PENDING or status == actionlib_msgs.msg.GoalStatus.RECALLING):
107		self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.RECALLED;
108		self.status_tracker.status.text = text
109		#on transition to a terminal state, we'll also set the handle destruction time
110		self.status_tracker.handle_destruction_time = rospy.Time.now()
111		self.action_server.publish_result(self.status_tracker.status, result);
112		elif(status == actionlib_msgs.msg.GoalStatus.ACTIVE or status == actionlib_msgs.msg.GoalStatus.PREEMPTING):
113		self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.PREEMPTED;
114		self.status_tracker.status.text = text
115		#on transition to a terminal state, we'll also set the handle destruction time
116		self.status_tracker.handle_destruction_time = rospy.Time.now()
117		self.action_server.publish_result(self.status_tracker.status, result);
118
	104	if status == actionlib_msgs.msg.GoalStatus.PENDING or status == actionlib_msgs.msg.GoalStatus.RECALLING:
	105	self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.RECALLED
	106	self.status_tracker.status.text = text
	107	# on transition to a terminal state, we'll also set the handle destruction time
	108	self.status_tracker.handle_destruction_time = rospy.Time.now()
	109	self.action_server.publish_result(self.status_tracker.status, result)
	110	elif status == actionlib_msgs.msg.GoalStatus.ACTIVE or status == actionlib_msgs.msg.GoalStatus.PREEMPTING:
	111	self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.PREEMPTED
	112	self.status_tracker.status.text = text
	113	# on transition to a terminal state, we'll also set the handle destruction time
	114	self.status_tracker.handle_destruction_time = rospy.Time.now()
	115	self.action_server.publish_result(self.status_tracker.status, result)
	116
119	117	else:
120	118	rospy.logerr("To transition to a cancelled state, the goal must be in a pending, recalling, active, or preempting state, it is currently in state: %d",
121		self.status_tracker.status.status);
122
123		else:
124		rospy.logerr("Attempt to set status on an uninitialized ServerGoalHandle");
125
	119	self.status_tracker.status.status)
	120
	121	else:
	122	rospy.logerr("Attempt to set status on an uninitialized ServerGoalHandle")
126	123
127	124	def set_rejected(self, result=None, text=""):
128	125	"""

130	127	* @param result Optionally, the user can pass in a result to be sent to any clients of the goal
131	128	"""
132	129	if not result:
133		result=self.get_default_result();
134
135		rospy.logdebug("Setting status to rejected on goal, id: %s, stamp: %.2f", self.get_goal_id().id, self.get_goal_id().stamp.to_sec());
136		if self.goal :
137		with self.action_server.lock:
138		status = self.status_tracker.status.status;
139		if(status == actionlib_msgs.msg.GoalStatus.PENDING or status == actionlib_msgs.msg.GoalStatus.RECALLING):
140		self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.REJECTED;
141		self.status_tracker.status.text = text
142		#on transition to a terminal state, we'll also set the handle destruction time
143		self.status_tracker.handle_destruction_time = rospy.Time.now()
144		self.action_server.publish_result(self.status_tracker.status, result);
145
	130	result = self.get_default_result()
	131
	132	rospy.logdebug("Setting status to rejected on goal, id: %s, stamp: %.2f", self.get_goal_id().id, self.get_goal_id().stamp.to_sec())
	133	if self.goal:
	134	with self.action_server.lock:
	135	status = self.status_tracker.status.status
	136	if status == actionlib_msgs.msg.GoalStatus.PENDING or status == actionlib_msgs.msg.GoalStatus.RECALLING:
	137	self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.REJECTED
	138	self.status_tracker.status.text = text
	139	# on transition to a terminal state, we'll also set the handle destruction time
	140	self.status_tracker.handle_destruction_time = rospy.Time.now()
	141	self.action_server.publish_result(self.status_tracker.status, result)
	142
146	143	else:
147	144	rospy.logerr("To transition to a rejected state, the goal must be in a pending or recalling state, it is currently in state: %d",
148		self.status_tracker.status.status);
149
150		else:
151		rospy.logerr("Attempt to set status on an uninitialized ServerGoalHandle");
152
	145	self.status_tracker.status.status)
	146
	147	else:
	148	rospy.logerr("Attempt to set status on an uninitialized ServerGoalHandle")
153	149
154	150	def set_aborted(self, result=None, text=""):
155	151	"""

157	153	@param result Optionally, the user can pass in a result to be sent to any clients of the goal
158	154	"""
159	155	if not result:
160		result=self.get_default_result();
161
162		rospy.logdebug("Setting status to aborted on goal, id: %s, stamp: %.2f", self.get_goal_id().id, self.get_goal_id().stamp.to_sec());
163		if self.goal:
164		with self.action_server.lock:
165		status = self.status_tracker.status.status;
	156	result = self.get_default_result()
	157
	158	rospy.logdebug("Setting status to aborted on goal, id: %s, stamp: %.2f", self.get_goal_id().id, self.get_goal_id().stamp.to_sec())
	159	if self.goal:
	160	with self.action_server.lock:
	161	status = self.status_tracker.status.status
166	162	if status == actionlib_msgs.msg.GoalStatus.PREEMPTING or status == actionlib_msgs.msg.GoalStatus.ACTIVE:
167		self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.ABORTED;
168		self.status_tracker.status.text = text
169		#on transition to a terminal state, we'll also set the handle destruction time
170		self.status_tracker.handle_destruction_time = rospy.Time.now()
171		self.action_server.publish_result(self.status_tracker.status, result);
172
	163	self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.ABORTED
	164	self.status_tracker.status.text = text
	165	# on transition to a terminal state, we'll also set the handle destruction time
	166	self.status_tracker.handle_destruction_time = rospy.Time.now()
	167	self.action_server.publish_result(self.status_tracker.status, result)
	168
173	169	else:
174	170	rospy.logerr("To transition to an aborted state, the goal must be in a preempting or active state, it is currently in state: %d",
175		status);
176
177		else:
178		rospy.logerr("Attempt to set status on an uninitialized ServerGoalHandle");
179
180
181		def set_succeeded(self,result=None, text=""):
	171	status)
	172
	173	else:
	174	rospy.logerr("Attempt to set status on an uninitialized ServerGoalHandle")
	175
	176	def set_succeeded(self, result=None, text=""):
182	177	"""
183	178	Set the status of the goal associated with the ServerGoalHandle to succeeded
184	179	@param result Optionally, the user can pass in a result to be sent to any clients of the goal
185	180	"""
186	181	if not result:
187		result=self.get_default_result();
	182	result = self.get_default_result()
188	183
189	184	rospy.logdebug("Setting status to succeeded on goal, id: %s, stamp: %.2f",
190		self.get_goal_id().id, self.get_goal_id().stamp.to_sec());
191		if self.goal:
192		with self.action_server.lock:
193		status = self.status_tracker.status.status;
194		if status == actionlib_msgs.msg.GoalStatus.PREEMPTING or status == actionlib_msgs.msg.GoalStatus.ACTIVE :
195		self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.SUCCEEDED;
196		self.status_tracker.status.text = text
197		#on transition to a terminal state, we'll also set the handle destruction time
198		self.status_tracker.handle_destruction_time = rospy.Time.now()
199		self.action_server.publish_result(self.status_tracker.status, result);
200
	185	self.get_goal_id().id, self.get_goal_id().stamp.to_sec())
	186	if self.goal:
	187	with self.action_server.lock:
	188	status = self.status_tracker.status.status
	189	if status == actionlib_msgs.msg.GoalStatus.PREEMPTING or status == actionlib_msgs.msg.GoalStatus.ACTIVE:
	190	self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.SUCCEEDED
	191	self.status_tracker.status.text = text
	192	# on transition to a terminal state, we'll also set the handle destruction time
	193	self.status_tracker.handle_destruction_time = rospy.Time.now()
	194	self.action_server.publish_result(self.status_tracker.status, result)
	195
201	196	else:
202	197	rospy.logerr("To transition to a succeeded state, the goal must be in a preempting or active state, it is currently in state: %d",
203		status);
204
205		else:
206		rospy.logerr("Attempt to set status on an uninitialized ServerGoalHandle");
207
208
209		def publish_feedback(self,feedback):
	198	status)
	199
	200	else:
	201	rospy.logerr("Attempt to set status on an uninitialized ServerGoalHandle")
	202
	203	def publish_feedback(self, feedback):
210	204	"""
211	205	Send feedback to any clients of the goal associated with this ServerGoalHandle
212	206	@param feedback The feedback to send to the client
213	207	"""
214		rospy.logdebug("Publishing feedback for goal, id: %s, stamp: %.2f",
215		self.get_goal_id().id, self.get_goal_id().stamp.to_sec());
216		if self.goal:
217		with self.action_server.lock:
218		self.action_server.publish_feedback(self.status_tracker.status, feedback);
219		else:
220		rospy.logerr("Attempt to publish feedback on an uninitialized ServerGoalHandle");
221
	208	rospy.logdebug("Publishing feedback for goal, id: %s, stamp: %.2f",
	209	self.get_goal_id().id, self.get_goal_id().stamp.to_sec())
	210	if self.goal:
	211	with self.action_server.lock:
	212	self.action_server.publish_feedback(self.status_tracker.status, feedback)
	213	else:
	214	rospy.logerr("Attempt to publish feedback on an uninitialized ServerGoalHandle")
222	215
223	216	def get_goal(self):
224	217	"""
225	218	Accessor for the goal associated with the ServerGoalHandle
226	219	@return A shared_ptr to the goal object
227	220	"""
228		#if we have a goal that is non-null
229		if self.goal:
230		#@todo Test that python reference counting automatically handles this.
231		#create the deleter for our goal subtype
232		#d = EnclosureDeleter(self.goal);
233		#weakref.ref(boost::shared_ptr<const Goal>(&(goal_->goal), d);
	221	# if we have a goal that is non-null
	222	if self.goal:
	223	# @todo Test that python reference counting automatically handles this.
	224	# create the deleter for our goal subtype
	225	# d = EnclosureDeleter(self.goal);
	226	# weakref.ref(boost::shared_ptr<const Goal>(&(goal_->goal), d);
234	227	return self.goal.goal
235
	228
236	229	return None
237
238	230
239	231	def get_goal_id(self):
240	232	"""

243	235	"""
244	236	if self.goal:
245	237	with self.action_server.lock:
246		return self.status_tracker.status.goal_id;
247		else:
248		rospy.logerr("Attempt to get a goal id on an uninitialized ServerGoalHandle");
249		return actionlib_msgs.msg.GoalID();
250
251
	238	return self.status_tracker.status.goal_id
	239	else:
	240	rospy.logerr("Attempt to get a goal id on an uninitialized ServerGoalHandle")
	241	return actionlib_msgs.msg.GoalID()
	242
252	243	def get_goal_status(self):
253	244	"""
254	245	Accessor for the status associated with the ServerGoalHandle

256	247	"""
257	248	if self.goal:
258	249	with self.action_server.lock:
259		return self.status_tracker.status;
260		else:
261		rospy.logerr("Attempt to get goal status on an uninitialized ServerGoalHandle");
262		return actionlib_msgs.msg.GoalStatus();
263
264
	250	return self.status_tracker.status
	251	else:
	252	rospy.logerr("Attempt to get goal status on an uninitialized ServerGoalHandle")
	253	return actionlib_msgs.msg.GoalStatus()
	254
265	255	def __eq__(self, other):
266	256	"""
267	257	Equals operator for ServerGoalHandles
268	258	@param other The ServerGoalHandle to compare to
269	259	@return True if the ServerGoalHandles refer to the same goal, false otherwise
270	260	"""
271
272		if( not self.goal or not other.goal):
273		return False;
274		my_id = self.get_goal_id();
275		their_id = other.get_goal_id();
276		return my_id.id == their_id.id;
	261
	262	if not self.goal or not other.goal:
	263	return False
	264	my_id = self.get_goal_id()
	265	their_id = other.get_goal_id()
	266	return my_id.id == their_id.id
277	267
278	268	def __ne__(self, other):
279	269	"""

281	271	@param other The ServerGoalHandle to compare to
282	272	@return True if the ServerGoalHandles refer to different goals, false otherwise
283	273	"""
284		if( not self.goal or not other.goal):
285		return True;
286		my_id = self.get_goal_id();
287		their_id = other.get_goal_id();
288
289		return my_id.id != their_id.id;
290
	274	if not self.goal or not other.goal:
	275	return True
	276	my_id = self.get_goal_id()
	277	their_id = other.get_goal_id()
	278
	279	return my_id.id != their_id.id
	280
	281	def __hash__(self):
	282	"""
	283	hash function for ServerGoalHandles
	284	@return hash of the goal ID
	285	"""
	286	return hash(self.get_goal_id().id)
291	287
292	288	def set_cancel_requested(self):
293	289	"""
294	290	A private method to set status to PENDING or RECALLING
295	291	@return True if the cancel request should be passed on to the user, false otherwise
296	292	"""
297		rospy.logdebug("Transisitoning to a cancel requested state on goal id: %s, stamp: %.2f",
298		self.get_goal_id().id, self.get_goal_id().stamp.to_sec());
299		if self.goal:
300		with self.action_server.lock:
301		status = self.status_tracker.status.status;
302		if (status == actionlib_msgs.msg.GoalStatus.PENDING):
303		self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.RECALLING;
304		self.action_server.publish_status();
305		return True;
306
307		if(status == actionlib_msgs.msg.GoalStatus.ACTIVE):
308		self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.PREEMPTING;
309		self.action_server.publish_status();
310		return True;
311
312		return False;
313
314
315
316
317
318
319
320
321
322
323
	293	rospy.logdebug("Transisitoning to a cancel requested state on goal id: %s, stamp: %.2f",
	294	self.get_goal_id().id, self.get_goal_id().stamp.to_sec())
	295	if self.goal:
	296	with self.action_server.lock:
	297	status = self.status_tracker.status.status
	298	if status == actionlib_msgs.msg.GoalStatus.PENDING:
	299	self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.RECALLING
	300	self.action_server.publish_status()
	301	return True
	302
	303	if status == actionlib_msgs.msg.GoalStatus.ACTIVE:
	304	self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.PREEMPTING
	305	self.action_server.publish_status()
	306	return True
	307
	308	return False

+16

-23

src/actionlib/simple_action_client.py less more

26	26	# POSSIBILITY OF SUCH DAMAGE.
27	27
28	28	# Author: Stuart Glaser
29
	29	import rospy
30	30	import threading
31		import time
32		import rospy
33		from rospy import Header
	31
34	32	from actionlib_msgs.msg import *
35		from action_client import ActionClient, CommState, get_name_of_constant
	33	from actionlib.action_client import ActionClient, CommState, get_name_of_constant
	34
36	35
37	36	class SimpleGoalState:
38	37	PENDING = 0
39	38	ACTIVE = 1
40	39	DONE = 2
	40
41	41	SimpleGoalState.to_string = classmethod(get_name_of_constant)
42	42
43	43

61	61	## timeout is interpreted as an infinite timeout.
62	62	##
63	63	## @return True if the server connected in the allocated time. False on timeout
64		def wait_for_server(self, timeout = rospy.Duration()):
	64	def wait_for_server(self, timeout=rospy.Duration()):
65	65	return self.action_client.wait_for_server(timeout)
66
67	66
68	67	## @brief Sends a goal to the ActionServer, and also registers callbacks
69	68	##

79	78	##
80	79	## @param feedback_cb Callback that gets called whenever feedback
81	80	## for this goal is received. Takes one parameter: the feedback.
82		def send_goal(self, goal, done_cb = None, active_cb = None, feedback_cb = None):
	81	def send_goal(self, goal, done_cb=None, active_cb=None, feedback_cb=None):
83	82	# destroys the old goal handle
84	83	self.stop_tracking_goal()
85	84

90	89	self.simple_state = SimpleGoalState.PENDING
91	90	self.gh = self.action_client.send_goal(goal, self._handle_transition, self._handle_feedback)
92	91
93
94	92	## @brief Sends a goal to the ActionServer, waits for the goal to complete, and preempts goal is necessary
95	93	##
96	94	## If a previous goal is already active when this is called. We simply forget

106	104	## @param preempt_timeout The time to wait for preemption to complete
107	105	##
108	106	## @return The goal's state.
109		def send_goal_and_wait(self, goal, execute_timeout = rospy.Duration(), preempt_timeout = rospy.Duration()):
	107	def send_goal_and_wait(self, goal, execute_timeout=rospy.Duration(), preempt_timeout=rospy.Duration()):
110	108	self.send_goal(goal)
111	109	if not self.wait_for_result(execute_timeout):
112	110	# preempt action
113	111	rospy.logdebug("Canceling goal")
114	112	self.cancel_goal()
115	113	if self.wait_for_result(preempt_timeout):
116		rospy.logdebug("Preempt finished within specified preempt_timeout [%.2f]", preempt_timeout.to_sec());
	114	rospy.logdebug("Preempt finished within specified preempt_timeout [%.2f]", preempt_timeout.to_sec())
117	115	else:
118		rospy.logdebug("Preempt didn't finish specified preempt_timeout [%.2f]", preempt_timeout.to_sec());
	116	rospy.logdebug("Preempt didn't finish specified preempt_timeout [%.2f]", preempt_timeout.to_sec())
119	117	return self.get_state()
120	118
121	119
122	120	## @brief Blocks until this goal transitions to done
123	121	## @param timeout Max time to block before returning. A zero timeout is interpreted as an infinite timeout.
124	122	## @return True if the goal finished. False if the goal didn't finish within the allocated timeout
125		def wait_for_result(self, timeout = rospy.Duration()):
	123	def wait_for_result(self, timeout=rospy.Duration()):
126	124	if not self.gh:
127	125	rospy.logerr("Called wait_for_goal_to_finish when no goal exists")
128	126	return False

145	143
146	144	return self.simple_state == SimpleGoalState.DONE
147	145
148
149	146	## @brief Gets the Result of the current goal
150	147	def get_result(self):
151	148	if not self.gh:

153	150	return None
154	151
155	152	return self.gh.get_result()
156
157	153
158	154	## @brief Get the state information for this goal
159	155	##

175	171
176	172	return status
177	173
178
179	174	## @brief Returns the current status text of the goal.
180	175	##
181	176	## The text is sent by the action server. It is designed to

202	197
203	198	## @brief Cancels all goals prior to a given timestamp
204	199	##
205		## This preempts all goals running on the action server for which the
	200	## This preempts all goals running on the action server for which the
206	201	## time stamp is earlier than the specified time stamp
207	202	## this message is serviced by the ActionServer.
208	203

258	253
259	254	def _handle_feedback(self, gh, feedback):
260	255	if not self.gh:
261		rospy.logerr("Got a feedback callback when we're not tracking a goal. (id: %s)" % \
262		gh.comm_state_machine.action_goal.goal_id.id)
	256	rospy.logerr("Got a feedback callback when we're not tracking a goal. (id: %s)" %
	257	gh.comm_state_machine.action_goal.goal_id.id)
263	258	return
264	259	if gh != self.gh:
265		rospy.logerr("Got a feedback callback on a goal handle that we're not tracking. %s vs %s" % \
266		(self.gh.comm_state_machine.action_goal.goal_id.id,
267		gh.comm_state_machine.action_goal.goal_id.id))
	260	rospy.logerr("Got a feedback callback on a goal handle that we're not tracking. %s vs %s" %
	261	(self.gh.comm_state_machine.action_goal.goal_id.id, gh.comm_state_machine.action_goal.goal_id.id))
268	262	return
269	263	if self.feedback_cb:
270	264	self.feedback_cb(feedback)
271	265
272
273	266	def _set_simple_state(self, state):
274	267	self.simple_state = state

+159

-170

src/actionlib/simple_action_server.py less more

27	27	#
28	28	# Author: Alexander Sorokin.
29	29	# Based on C++ simple_action_server.h by Eitan Marder-Eppstein
30
31	30	import rospy
32
33	31	import threading
34	32	import traceback
35	33
36	34	from actionlib_msgs.msg import *
37	35
38	36	from actionlib import ActionServer
39		from actionlib.server_goal_handle import ServerGoalHandle;
	37	from actionlib.server_goal_handle import ServerGoalHandle
	38
40	39
41	40	def nop_cb(goal_handle):
42	41	pass

59	58	## a new goal is received, allowing users to have blocking callbacks.
60	59	## Adding an execute callback also deactivates the goalCallback.
61	60	## @param auto_start A boolean value that tells the ActionServer wheteher or not to start publishing as soon as it comes up. THIS SHOULD ALWAYS BE SET TO FALSE TO AVOID RACE CONDITIONS and start() should be called after construction of the server.
62		def __init__(self, name, ActionSpec, execute_cb = None, auto_start = True):
	61	def __init__(self, name, ActionSpec, execute_cb=None, auto_start=True):
63	62
64	63	self.new_goal = False
65	64	self.preempt_request = False
66	65	self.new_goal_preempt_request = False
67	66
68		self.execute_callback = execute_cb;
69		self.goal_callback = None;
70		self.preempt_callback = None;
	67	self.execute_callback = execute_cb
	68	self.goal_callback = None
	69	self.preempt_callback = None
71	70
72	71	self.need_to_terminate = False
73		self.terminate_mutex = threading.RLock();
	72	self.terminate_mutex = threading.RLock()
74	73
75	74	# since the internal_goal/preempt_callbacks are invoked from the
76	75	# ActionServer while holding the self.action_server.lock
77	76	# self.lock must always be locked after the action server lock
78	77	# to avoid an inconsistent lock acquisition order
79		self.lock = threading.RLock();
80
81		self.execute_condition = threading.Condition(self.lock);
82
83		self.current_goal = ServerGoalHandle();
84		self.next_goal = ServerGoalHandle();
	78	self.lock = threading.RLock()
	79
	80	self.execute_condition = threading.Condition(self.lock)
	81
	82	self.current_goal = ServerGoalHandle()
	83	self.next_goal = ServerGoalHandle()
85	84
86	85	if self.execute_callback:
87		self.execute_thread = threading.Thread(None, self.executeLoop);
88		self.execute_thread.start();
	86	self.execute_thread = threading.Thread(None, self.executeLoop)
	87	self.execute_thread.start()
89	88	else:
90	89	self.execute_thread = None
91	90
92	91	#create the action server
93		self.action_server = ActionServer(name, ActionSpec, self.internal_goal_callback,self.internal_preempt_callback,auto_start);
94
	92	self.action_server = ActionServer(name, ActionSpec, self.internal_goal_callback,self.internal_preempt_callback, auto_start)
95	93
96	94	def __del__(self):
97	95	if hasattr(self, 'execute_callback') and self.execute_callback:
98	96	with self.terminate_mutex:
99		self.need_to_terminate = True;
100
101		assert(self.execute_thread);
102		self.execute_thread.join();
103
	97	self.need_to_terminate = True
	98
	99	assert(self.execute_thread)
	100	self.execute_thread.join()
104	101
105	102	## @brief Accepts a new goal when one is available The status of this
106	103	## goal is set to active upon acceptance, and the status of any

114	111	def accept_new_goal(self):
115	112	with self.action_server.lock, self.lock:
116	113	if not self.new_goal or not self.next_goal.get_goal():
117		rospy.logerr("Attempting to accept the next goal when a new goal is not available");
118		return None;
119
120		#check if we need to send a preempted message for the goal that we're currently pursuing
	114	rospy.logerr("Attempting to accept the next goal when a new goal is not available")
	115	return None
	116
	117	# check if we need to send a preempted message for the goal that we're currently pursuing
121	118	if self.is_active() and self.current_goal.get_goal() and self.current_goal != self.next_goal:
122		self.current_goal.set_canceled(None, "This goal was canceled because another goal was received by the simple action server");
123
124		rospy.logdebug("Accepting a new goal");
125
126		#accept the next goal
127		self.current_goal = self.next_goal;
128		self.new_goal = False;
129
130		#set preempt to request to equal the preempt state of the new goal
131		self.preempt_request = self.new_goal_preempt_request;
132		self.new_goal_preempt_request = False;
133
134		#set the status of the current goal to be active
135		self.current_goal.set_accepted("This goal has been accepted by the simple action server");
136
137		return self.current_goal.get_goal();
138
	119	self.current_goal.set_canceled(None, "This goal was canceled because another goal was received by the simple action server")
	120
	121	rospy.logdebug("Accepting a new goal")
	122
	123	# accept the next goal
	124	self.current_goal = self.next_goal
	125	self.new_goal = False
	126
	127	# set preempt to request to equal the preempt state of the new goal
	128	self.preempt_request = self.new_goal_preempt_request
	129	self.new_goal_preempt_request = False
	130
	131	# set the status of the current goal to be active
	132	self.current_goal.set_accepted("This goal has been accepted by the simple action server")
	133
	134	return self.current_goal.get_goal()
139	135
140	136	## @brief Allows polling implementations to query about the availability of a new goal
141	137	## @return True if a new goal is available, false otherwise
142	138	def is_new_goal_available(self):
143		return self.new_goal;
144
	139	return self.new_goal
145	140
146	141	## @brief Allows polling implementations to query about preempt requests
147	142	## @return True if a preempt is requested, false otherwise
148	143	def is_preempt_requested(self):
149		return self.preempt_request;
	144	return self.preempt_request
150	145
151	146	## @brief Allows polling implementations to query about the status of the current goal
152	147	## @return True if a goal is active, false otherwise
153	148	def is_active(self):
154		if not self.current_goal.get_goal():
155		return False;
156
157		status = self.current_goal.get_goal_status().status;
158		return status == actionlib_msgs.msg.GoalStatus.ACTIVE or status == actionlib_msgs.msg.GoalStatus.PREEMPTING;
	149	if not self.current_goal.get_goal():
	150	return False
	151
	152	status = self.current_goal.get_goal_status().status
	153	return status == actionlib_msgs.msg.GoalStatus.ACTIVE or status == actionlib_msgs.msg.GoalStatus.PREEMPTING
159	154
160	155	## @brief Sets the status of the active goal to succeeded
161	156	## @param result An optional result to send back to any clients of the goal
162		def set_succeeded(self,result=None, text=""):
163		with self.action_server.lock, self.lock:
164		if not result:
165		result=self.get_default_result();
166		self.current_goal.set_succeeded(result, text);
	157	def set_succeeded(self, result=None, text=""):
	158	with self.action_server.lock, self.lock:
	159	if not result:
	160	result = self.get_default_result()
	161	self.current_goal.set_succeeded(result, text)
167	162
168	163	## @brief Sets the status of the active goal to aborted
169	164	## @param result An optional result to send back to any clients of the goal
170		def set_aborted(self, result = None, text=""):
	165	def set_aborted(self, result=None, text=""):
171	166	with self.action_server.lock, self.lock:
172	167	if not result:
173		result=self.get_default_result();
174		self.current_goal.set_aborted(result, text);
	168	result = self.get_default_result()
	169	self.current_goal.set_aborted(result, text)
175	170
176	171	## @brief Publishes feedback for a given goal
177	172	## @param feedback Shared pointer to the feedback to publish
178		def publish_feedback(self,feedback):
179		self.current_goal.publish_feedback(feedback);
180
	173	def publish_feedback(self, feedback):
	174	self.current_goal.publish_feedback(feedback)
181	175
182	176	def get_default_result(self):
183		return self.action_server.ActionResultType();
	177	return self.action_server.ActionResultType()
184	178
185	179	## @brief Sets the status of the active goal to preempted
186	180	## @param result An optional result to send back to any clients of the goal
187		def set_preempted(self,result=None, text=""):
	181	def set_preempted(self, result=None, text=""):
188	182	if not result:
189		result=self.get_default_result();
	183	result = self.get_default_result()
190	184	with self.action_server.lock, self.lock:
191		rospy.logdebug("Setting the current goal as canceled");
192		self.current_goal.set_canceled(result, text);
	185	rospy.logdebug("Setting the current goal as canceled")
	186	self.current_goal.set_canceled(result, text)
193	187
194	188	## @brief Allows users to register a callback to be invoked when a new goal is available
195	189	## @param cb The callback to be invoked
196		def register_goal_callback(self,cb):
	190	def register_goal_callback(self, cb):
197	191	if self.execute_callback:
198		rospy.logwarn("Cannot call SimpleActionServer.register_goal_callback() because an executeCallback exists. Not going to register it.");
	192	rospy.logwarn("Cannot call SimpleActionServer.register_goal_callback() because an executeCallback exists. Not going to register it.")
199	193	else:
200		self.goal_callback = cb;
	194	self.goal_callback = cb
201	195
202	196	## @brief Allows users to register a callback to be invoked when a new preempt request is available
203	197	## @param cb The callback to be invoked
204	198	def register_preempt_callback(self, cb):
205		self.preempt_callback = cb;
206
	199	self.preempt_callback = cb
207	200
208	201	## @brief Explicitly start the action server, used it auto_start is set to false
209	202	def start(self):
210		self.action_server.start();
211
	203	self.action_server.start()
212	204
213	205	## @brief Callback for when the ActionServer receives a new goal and passes it on
214	206	def internal_goal_callback(self, goal):
215		self.execute_condition.acquire();
216
217		try:
218		rospy.logdebug("A new goal %shas been recieved by the single goal action server",goal.get_goal_id().id);
219
220		#check that the timestamp is past that of the current goal and the next goal
221		if((not self.current_goal.get_goal() or goal.get_goal_id().stamp >= self.current_goal.get_goal_id().stamp)
222		and (not self.next_goal.get_goal() or goal.get_goal_id().stamp >= self.next_goal.get_goal_id().stamp)):
223		#if next_goal has not been accepted already... its going to get bumped, but we need to let the client know we're preempting
224		if(self.next_goal.get_goal() and (not self.current_goal.get_goal() or self.next_goal != self.current_goal)):
225		self.next_goal.set_canceled(None, "This goal was canceled because another goal was received by the simple action server");
226
227		self.next_goal = goal;
228		self.new_goal = True;
229		self.new_goal_preempt_request = False;
230
231		#if the server is active, we'll want to call the preempt callback for the current goal
232		if(self.is_active()):
233		self.preempt_request = True;
234		#if the user has registered a preempt callback, we'll call it now
235		if(self.preempt_callback):
236		self.preempt_callback();
237
238		#if the user has defined a goal callback, we'll call it now
239		if self.goal_callback:
240		self.goal_callback();
241
242		#Trigger runLoop to call execute()
243		self.execute_condition.notify();
244		self.execute_condition.release();
245		else:
246		#the goal requested has already been preempted by a different goal, so we're not going to execute it
247		goal.set_canceled(None, "This goal was canceled because another goal was received by the simple action server");
248		self.execute_condition.release();
249		except Exception, e:
250		rospy.logerr("SimpleActionServer.internal_goal_callback - exception %s",str(e))
251		self.execute_condition.release();
	207	self.execute_condition.acquire()
	208
	209	try:
	210	rospy.logdebug("A new goal %shas been recieved by the single goal action server", goal.get_goal_id().id)
	211
	212	# check that the timestamp is past that of the current goal and the next goal
	213	if((not self.current_goal.get_goal() or goal.get_goal_id().stamp >= self.current_goal.get_goal_id().stamp)
	214	and (not self.next_goal.get_goal() or goal.get_goal_id().stamp >= self.next_goal.get_goal_id().stamp)):
	215	# if next_goal has not been accepted already... its going to get bumped, but we need to let the client know we're preempting
	216	if(self.next_goal.get_goal() and (not self.current_goal.get_goal() or self.next_goal != self.current_goal)):
	217	self.next_goal.set_canceled(None, "This goal was canceled because another goal was received by the simple action server")
	218
	219	self.next_goal = goal
	220	self.new_goal = True
	221	self.new_goal_preempt_request = False
	222
	223	# if the server is active, we'll want to call the preempt callback for the current goal
	224	if(self.is_active()):
	225	self.preempt_request = True
	226	# if the user has registered a preempt callback, we'll call it now
	227	if(self.preempt_callback):
	228	self.preempt_callback()
	229
	230	# if the user has defined a goal callback, we'll call it now
	231	if self.goal_callback:
	232	self.goal_callback()
	233
	234	# Trigger runLoop to call execute()
	235	self.execute_condition.notify()
	236	self.execute_condition.release()
	237	else:
	238	# the goal requested has already been preempted by a different goal, so we're not going to execute it
	239	goal.set_canceled(None, "This goal was canceled because another goal was received by the simple action server")
	240	self.execute_condition.release()
	241	except Exception as e:
	242	rospy.logerr("SimpleActionServer.internal_goal_callback - exception %s", str(e))
	243	self.execute_condition.release()
252	244
253	245	## @brief Callback for when the ActionServer receives a new preempt and passes it on
254		def internal_preempt_callback(self,preempt):
255		with self.lock:
256		rospy.logdebug("A preempt has been received by the SimpleActionServer");
257
258		#if the preempt is for the current goal, then we'll set the preemptRequest flag and call the user's preempt callback
259		if(preempt == self.current_goal):
260		rospy.logdebug("Setting preempt_request bit for the current goal to TRUE and invoking callback");
261		self.preempt_request = True;
262
263		#if the user has registered a preempt callback, we'll call it now
264		if(self.preempt_callback):
265		self.preempt_callback();
266		#if the preempt applies to the next goal, we'll set the preempt bit for that
267		elif(preempt == self.next_goal):
268		rospy.logdebug("Setting preempt request bit for the next goal to TRUE");
269		self.new_goal_preempt_request = True;
	246	def internal_preempt_callback(self, preempt):
	247	with self.lock:
	248	rospy.logdebug("A preempt has been received by the SimpleActionServer")
	249
	250	#if the preempt is for the current goal, then we'll set the preemptRequest flag and call the user's preempt callback
	251	if(preempt == self.current_goal):
	252	rospy.logdebug("Setting preempt_request bit for the current goal to TRUE and invoking callback")
	253	self.preempt_request = True
	254
	255	#if the user has registered a preempt callback, we'll call it now
	256	if(self.preempt_callback):
	257	self.preempt_callback()
	258	#if the preempt applies to the next goal, we'll set the preempt bit for that
	259	elif(preempt == self.next_goal):
	260	rospy.logdebug("Setting preempt request bit for the next goal to TRUE")
	261	self.new_goal_preempt_request = True
270	262
271	263	## @brief Called from a separate thread to call blocking execute calls
272	264	def executeLoop(self):
273		loop_duration = rospy.Duration.from_sec(.1);
274
275		while (not rospy.is_shutdown()):
276		rospy.logdebug("SAS: execute");
277
278		with self.terminate_mutex:
279		if (self.need_to_terminate):
280		break;
281
282		# the following checks (is_active, is_new_goal_available)
283		# are performed without locking
284		# the worst thing that might happen in case of a race
285		# condition is a warning/error message on the console
286		if (self.is_active()):
287		rospy.logerr("Should never reach this code with an active goal");
288		return
289
290		if (self.is_new_goal_available()):
291		# accept_new_goal() is performing its own locking
292		goal = self.accept_new_goal();
293		if not self.execute_callback:
294		rospy.logerr("execute_callback_ must exist. This is a bug in SimpleActionServer");
295		return
296
297		try:
298		self.execute_callback(goal)
299
300		if self.is_active():
301		rospy.logwarn("Your executeCallback did not set the goal to a terminal status. " +
302		"This is a bug in your ActionServer implementation. Fix your code! "+
303		"For now, the ActionServer will set this goal to aborted");
304		self.set_aborted(None, "No terminal state was set.");
305		except Exception, ex:
306		rospy.logerr("Exception in your execute callback: %s\n%s", str(ex),
307		traceback.format_exc())
308		self.set_aborted(None, "Exception in execute callback: %s" % str(ex))
309
310		with self.execute_condition:
311		self.execute_condition.wait(loop_duration.to_sec());
312
313
314
	265	loop_duration = rospy.Duration.from_sec(.1)
	266
	267	while (not rospy.is_shutdown()):
	268	rospy.logdebug("SAS: execute")
	269
	270	with self.terminate_mutex:
	271	if (self.need_to_terminate):
	272	break
	273
	274	# the following checks (is_active, is_new_goal_available)
	275	# are performed without locking
	276	# the worst thing that might happen in case of a race
	277	# condition is a warning/error message on the console
	278	if (self.is_active()):
	279	rospy.logerr("Should never reach this code with an active goal")
	280	return
	281
	282	if (self.is_new_goal_available()):
	283	# accept_new_goal() is performing its own locking
	284	goal = self.accept_new_goal()
	285	if not self.execute_callback:
	286	rospy.logerr("execute_callback_ must exist. This is a bug in SimpleActionServer")
	287	return
	288
	289	try:
	290	self.execute_callback(goal)
	291
	292	if self.is_active():
	293	rospy.logwarn("Your executeCallback did not set the goal to a terminal status. " +
	294	"This is a bug in your ActionServer implementation. Fix your code! " +
	295	"For now, the ActionServer will set this goal to aborted")
	296	self.set_aborted(None, "No terminal state was set.")
	297	except Exception as ex:
	298	rospy.logerr("Exception in your execute callback: %s\n%s", str(ex),
	299	traceback.format_exc())
	300	self.set_aborted(None, "Exception in execute callback: %s" % str(ex))
	301
	302	with self.execute_condition:
	303	self.execute_condition.wait(loop_duration.to_sec())

+21

-23

src/actionlib/status_tracker.py less more

0	0	# Copyright (c) 2009, Willow Garage, Inc.
1	1	# All rights reserved.
2		#
	2	#
3	3	# Redistribution and use in source and binary forms, with or without
4	4	# modification, are permitted provided that the following conditions are met:
5		#
	5	#
6	6	# * Redistributions of source code must retain the above copyright
7	7	# notice, this list of conditions and the following disclaimer.
8	8	# * Redistributions in binary form must reproduce the above copyright

11	11	# * Neither the name of the Willow Garage, Inc. nor the names of its
12	12	# contributors may be used to endorse or promote products derived from
13	13	# this software without specific prior written permission.
14		#
	14	#
15	15	# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16	16	# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17	17	# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

24	24	# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25	25	# POSSIBILITY OF SUCH DAMAGE.
26	26
27		# Author: Alexander Sorokin.
	27	# Author: Alexander Sorokin.
28	28	# Based on C++ goal_id_generator.h/cpp
29
30	29	import rospy
31	30
32		import actionlib
33		import actionlib_msgs.msg
34		from actionlib import goal_id_generator
	31	import actionlib_msgs.msg
	32	from actionlib import goal_id_generator
35	33
36	34
37	35	class StatusTracker:

45	43	"""
46	44	@brief create status tracker. Either pass goal_id and status OR goal
47	45	"""
48		self.goal = None ;
49		self.handle_tracker = None;
50		self.status = actionlib_msgs.msg.GoalStatus();
	46	self.goal = None
	47	self.handle_tracker = None
	48	self.status = actionlib_msgs.msg.GoalStatus()
51	49
52		self.handle_destruction_time = rospy.Time();
	50	self.handle_destruction_time = rospy.Time()
53	51
54		self.id_generator = goal_id_generator.GoalIDGenerator();
	52	self.id_generator = goal_id_generator.GoalIDGenerator()
55	53
56	54	if goal_id:
57		#set the goal id and status appropriately
58		self.status.goal_id = goal_id;
59		self.status.status = status;
	55	# set the goal id and status appropriately
	56	self.status.goal_id = goal_id
	57	self.status.status = status
60	58	else:
61	59	self.goal = goal
62		self.status.goal_id = goal.goal_id;
	60	self.status.goal_id = goal.goal_id
63	61
64		#initialize the status of the goal to pending
65		self.status.status = actionlib_msgs.msg.GoalStatus.PENDING;
	62	# initialize the status of the goal to pending
	63	self.status.status = actionlib_msgs.msg.GoalStatus.PENDING
66	64
67		#if the goal id is zero, then we need to make up an id for the goal
	65	# if the goal id is zero, then we need to make up an id for the goal
68	66	if self.status.goal_id.id == "":
69		self.status.goal_id = self.id_generator.generate_ID();
	67	self.status.goal_id = self.id_generator.generate_ID()
70	68
71		#if the timestamp of the goal is zero, then we'll set it to now()
	69	# if the timestamp of the goal is zero, then we'll set it to now()
72	70	if self.status.goal_id.stamp == rospy.Time():
73		self.status.goal_id.stamp = rospy.Time.now();
	71	self.status.goal_id.stamp = rospy.Time.now()

+16

-13

tools/axclient.py less more

30	30	#* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31	31	#* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32	32	#* POSSIBILITY OF SUCH DAMAGE.
33		#*
	33	#*
34	34	#* Author: Eitan Marder-Eppstein
35	35	#***********************************************************
36	36	"""

169	169	self.goal_st_bx = wx.StaticBox(self.frame, -1, "Goal")
170	170	self.goal_st = wx.StaticBoxSizer(self.goal_st_bx, wx.VERTICAL)
171	171	self.goal_st.Add(self.goal, 1, wx.EXPAND)
172
	172
173	173	self.feedback = wx.TextCtrl(self.frame, -1, style=(wx.TE_MULTILINE \|
174	174	wx.TE_READONLY))
175	175	self.feedback_st_bx = wx.StaticBox(self.frame, -1, "Feedback")

228	228	# parser.add_option("-t","--test",action="store_true", dest="test",default=False,
229	229	# help="A testing flag")
230	230	# parser.add_option("-v","--var",action="store",type="string", dest="var",default="blah")
231
	231
232	232	(options, args) = parser.parse_args(rospy.myargv())
233	233
234		if (len(args) != 2):
235		parser.error("You must specify the action topic name Eg: ./axclient.py my_action_topic")
236
237		# get action type from topic
238		topic_type = rostopic._get_topic_type("%s/goal"%args[1])[0]
239		# remove "Goal" string from action type
240		assert("Goal" in topic_type)
241		topic_type = topic_type[0:len(topic_type)-4]
242
	234	if (len(args) == 2):
	235	# get action type via rostopic
	236	topic_type = rostopic._get_topic_type("%s/goal"%args[1])[0]
	237	# remove "Goal" string from action type
	238	assert("Goal" in topic_type)
	239	topic_type = topic_type[0:len(topic_type)-4]
	240	elif (len(args) == 3):
	241	topic_type = args[2]
	242	print(topic_type)
	243	assert("Action" in topic_type)
	244	else:
	245	parser.error("You must specify the action topic name (and optionally type) Eg: ./axclient.py action_topic actionlib/TwoIntsAction ")
	246
243	247	action = DynamicAction(topic_type)
244
245	248	app = AXClientApp(action, args[1])
246	249	app.MainLoop()
247	250	app.OnQuit()

+18

-14

tools/axserver.py less more

71	71	self.status.SetLabel("Waiting For Goal...")
72	72	self.send_feedback.Disable()
73	73	self.succeed.Disable()
74		self.abort.Disable()
75		self.preempt.Disable()
	74	self.abort.Disable()
	75	self.preempt.Disable()
76	76
77	77	self.goal.SetValue("")
78	78

82	82	self.send_feedback.Enable()
83	83	self.succeed.Enable()
84	84	self.abort.Enable()
85		self.preempt.Enable()
	85	self.preempt.Enable()
86	86
87	87	try:
88	88	self.goal.SetValue(to_yaml(goal))
89	89	except UnicodeDecodeError:
90	90	self.goal.SetValue("Cannot display goal due to unprintable characters")
91	91
	92	def set_preempt_requested(self):
	93	self.status_bg.SetBackgroundColour(wx.Colour(0, 200, 200))
	94	self.status.SetLabel("Preempt requested...")
	95
92	96	def execute(self, goal):
93	97
94	98	wx.CallAfter(self.set_goal, goal)
95
96	99	self.condition.acquire()
97	100
98	101	self.result_msg = None
99	102	self.feedback_msg = None
100	103	self.execute_type = None
101
	104
102	105	while self.execute_type is None or self.execute_type == SEND_FEEDBACK:
103
104	106	self.result_msg = None
105	107	self.feedback_msg = None
106	108	self.execute_type = None
107
	109
108	110	while self.execute_type is None:
109		self.condition.wait()
	111	if self.server.is_preempt_requested():
	112	wx.CallAfter(self.set_preempt_requested)
	113	self.condition.wait(1.0)
110	114
111	115	if self.execute_type == SEND_FEEDBACK:
112	116	if self.feedback_msg is not None:

128	132
129	133	def on_feedback(self, event):
130	134	self.condition.acquire()
131
	135
132	136	try:
133	137	self.feedback_msg = yaml_msg_str(self.action_type.feedback,
134	138	self.feedback.GetValue())
135	139	buff = StringIO()
136	140	self.feedback_msg.serialize(buff)
137
	141
138	142	self.execute_type = SEND_FEEDBACK
139	143	self.condition.notify()
140	144	except roslib.message.SerializationError, e:

147	151
148	152	def on_succeed(self, event):
149	153	self.condition.acquire()
150
	154
151	155	try:
152	156	self.result_msg = yaml_msg_str(self.action_type.result, self.result.GetValue())
153	157	buff = StringIO()
154	158	self.result_msg.serialize(buff)
155
	159
156	160	self.execute_type = SUCCEED
157	161	self.condition.notify()
158	162	except roslib.message.SerializationError, e:

190	194	self.goal_st_bx = wx.StaticBox(self.frame, -1, "Goal")
191	195	self.goal_st = wx.StaticBoxSizer(self.goal_st_bx, wx.VERTICAL)
192	196	self.goal_st.Add(self.goal, 1, wx.EXPAND)
193
	197
194	198	self.feedback = wx.TextCtrl(self.frame, -1, style=wx.TE_MULTILINE)
195	199	self.feedback.SetValue(to_yaml(tmp_feedback))
196	200	self.feedback_st_bx = wx.StaticBox(self.frame, -1, "Feedback")

244	248	# parser.add_option("-t","--test",action="store_true", dest="test",default=False,
245	249	# help="A testing flag")
246	250	# parser.add_option("-v","--var",action="store",type="string", dest="var",default="blah")
247
	251
248	252	(options, args) = parser.parse_args(rospy.myargv())
249	253
250	254	if (len(args) != 3):