comm_state_machine_imp.h
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* This file has the template implementation for CommStateMachine. It should be included with the
36  * class definition.
37  */
38 #ifndef ACTIONLIB__CLIENT__COMM_STATE_MACHINE_IMP_H_
39 #define ACTIONLIB__CLIENT__COMM_STATE_MACHINE_IMP_H_
40 
41 #include <vector>
42 
43 #include "ros/console.h"
44 
45 namespace actionlib
46 {
47 
48 template<class ActionSpec>
49 CommStateMachine<ActionSpec>::CommStateMachine(const ActionGoalConstPtr & action_goal,
50  TransitionCallback transition_cb,
51  FeedbackCallback feedback_cb)
52 : state_(CommState::WAITING_FOR_GOAL_ACK)
53 {
54  assert(action_goal);
55  action_goal_ = action_goal;
56  transition_cb_ = transition_cb;
57  feedback_cb_ = feedback_cb;
58  // transitionToState( CommState::WAITING_FOR_GOAL_ACK );
59 }
60 
61 template<class ActionSpec>
62 typename CommStateMachine<ActionSpec>::ActionGoalConstPtr CommStateMachine<ActionSpec>::
63 getActionGoal() const
64 {
65  return action_goal_;
66 }
67 
68 template<class ActionSpec>
70 {
71  return state_;
72 }
73 
74 template<class ActionSpec>
75 actionlib_msgs::GoalStatus CommStateMachine<ActionSpec>::getGoalStatus() const
76 {
77  return latest_goal_status_;
78 }
79 
80 template<class ActionSpec>
82 const
83 {
84  ResultConstPtr result;
85  if (latest_result_) {
87  result = ResultConstPtr(&(latest_result_->result), d);
88  }
89  return result;
90 }
91 
92 template<class ActionSpec>
94 {
95  setCommState(CommState(state));
96 }
97 
98 template<class ActionSpec>
100 {
101  ROS_DEBUG_NAMED("actionlib", "Transitioning CommState from %s to %s",
102  state_.toString().c_str(), state.toString().c_str());
103  state_ = state;
104 }
105 
106 template<class ActionSpec>
107 const actionlib_msgs::GoalStatus * CommStateMachine<ActionSpec>::findGoalStatus(
108  const std::vector<actionlib_msgs::GoalStatus> & status_vec) const
109 {
110  for (unsigned int i = 0; i < status_vec.size(); i++) {
111  if (status_vec[i].goal_id.id == action_goal_->goal_id.id) {
112  return &status_vec[i];
113  }
114  }
115  return nullptr;
116 }
117 
118 template<class ActionSpec>
120  const ActionFeedbackConstPtr & action_feedback)
121 {
122  // Check if this feedback is for us
123  if (action_goal_->goal_id.id != action_feedback->status.goal_id.id) {
124  return;
125  }
126 
127  if (feedback_cb_) {
128  EnclosureDeleter<const ActionFeedback> d(action_feedback);
129  FeedbackConstPtr feedback(&(action_feedback->feedback), d);
130  feedback_cb_(gh, feedback);
131  }
132 }
133 
134 template<class ActionSpec>
135 void CommStateMachine<ActionSpec>::updateResult(GoalHandleT & gh,
136  const ActionResultConstPtr & action_result)
137 {
138  // Check if this feedback is for us
139  if (action_goal_->goal_id.id != action_result->status.goal_id.id) {
140  return;
141  }
142  latest_goal_status_ = action_result->status;
143  latest_result_ = action_result;
144  switch (state_.state_) {
146  case CommState::PENDING:
147  case CommState::ACTIVE:
152  {
153  // A little bit of hackery to call all the right state transitions before processing result
154  actionlib_msgs::GoalStatusArrayPtr status_array(new actionlib_msgs::GoalStatusArray());
155  status_array->status_list.push_back(action_result->status);
156  updateStatus(gh, status_array);
157 
158  transitionToState(gh, CommState::DONE);
159  break;
160  }
161  case CommState::DONE:
162  ROS_ERROR_NAMED("actionlib", "Got a result when we were already in the DONE state"); break;
163  default:
164  ROS_ERROR_NAMED("actionlib", "In a funny comm state: %u", state_.state_); break;
165  }
166 }
167 
168 template<class ActionSpec>
170  const actionlib_msgs::GoalStatusArrayConstPtr & status_array)
171 {
172  const actionlib_msgs::GoalStatus * goal_status = findGoalStatus(status_array->status_list);
173 
174  // It's possible to receive old GoalStatus messages over the wire, even after receiving Result with a terminal state.
175  // Thus, we want to ignore all status that we get after we're done, because it is irrelevant. (See trac #2721)
176  if (state_ == CommState::DONE) {
177  return;
178  }
179 
180  if (goal_status) {
181  latest_goal_status_ = *goal_status;
182  } else {
183  if (state_ != CommState::WAITING_FOR_GOAL_ACK &&
184  state_ != CommState::WAITING_FOR_RESULT &&
185  state_ != CommState::DONE)
186  {
187  processLost(gh);
188  }
189  return;
190  }
191 
192  switch (state_.state_) {
194  {
195  if (goal_status) {
196  switch (goal_status->status) {
197  case actionlib_msgs::GoalStatus::PENDING:
198  transitionToState(gh, CommState::PENDING);
199  break;
200  case actionlib_msgs::GoalStatus::ACTIVE:
201  transitionToState(gh, CommState::ACTIVE);
202  break;
203  case actionlib_msgs::GoalStatus::PREEMPTED:
204  transitionToState(gh, CommState::ACTIVE);
205  transitionToState(gh, CommState::PREEMPTING);
206  transitionToState(gh, CommState::WAITING_FOR_RESULT);
207  break;
208  case actionlib_msgs::GoalStatus::SUCCEEDED:
209  transitionToState(gh, CommState::ACTIVE);
210  transitionToState(gh, CommState::WAITING_FOR_RESULT);
211  break;
212  case actionlib_msgs::GoalStatus::ABORTED:
213  transitionToState(gh, CommState::ACTIVE);
214  transitionToState(gh, CommState::WAITING_FOR_RESULT);
215  break;
216  case actionlib_msgs::GoalStatus::REJECTED:
217  transitionToState(gh, CommState::PENDING);
218  transitionToState(gh, CommState::WAITING_FOR_RESULT);
219  break;
220  case actionlib_msgs::GoalStatus::RECALLED:
221  transitionToState(gh, CommState::PENDING);
222  transitionToState(gh, CommState::WAITING_FOR_RESULT);
223  break;
224  case actionlib_msgs::GoalStatus::PREEMPTING:
225  transitionToState(gh, CommState::ACTIVE);
226  transitionToState(gh, CommState::PREEMPTING);
227  break;
228  case actionlib_msgs::GoalStatus::RECALLING:
229  transitionToState(gh, CommState::PENDING);
230  transitionToState(gh, CommState::RECALLING);
231  break;
232  default:
233  ROS_ERROR_NAMED("actionlib",
234  "BUG: Got an unknown status from the ActionServer. status = %u",
235  goal_status->status);
236  break;
237  }
238  }
239  break;
240  }
241  case CommState::PENDING:
242  {
243  switch (goal_status->status) {
244  case actionlib_msgs::GoalStatus::PENDING:
245  break;
246  case actionlib_msgs::GoalStatus::ACTIVE:
247  transitionToState(gh, CommState::ACTIVE);
248  break;
249  case actionlib_msgs::GoalStatus::PREEMPTED:
250  transitionToState(gh, CommState::ACTIVE);
251  transitionToState(gh, CommState::PREEMPTING);
252  transitionToState(gh, CommState::WAITING_FOR_RESULT);
253  break;
254  case actionlib_msgs::GoalStatus::SUCCEEDED:
255  transitionToState(gh, CommState::ACTIVE);
256  transitionToState(gh, CommState::WAITING_FOR_RESULT);
257  break;
258  case actionlib_msgs::GoalStatus::ABORTED:
259  transitionToState(gh, CommState::ACTIVE);
260  transitionToState(gh, CommState::WAITING_FOR_RESULT);
261  break;
262  case actionlib_msgs::GoalStatus::REJECTED:
263  transitionToState(gh, CommState::WAITING_FOR_RESULT);
264  break;
265  case actionlib_msgs::GoalStatus::RECALLED:
266  transitionToState(gh, CommState::RECALLING);
267  transitionToState(gh, CommState::WAITING_FOR_RESULT);
268  break;
269  case actionlib_msgs::GoalStatus::PREEMPTING:
270  transitionToState(gh, CommState::ACTIVE);
271  transitionToState(gh, CommState::PREEMPTING);
272  break;
273  case actionlib_msgs::GoalStatus::RECALLING:
274  transitionToState(gh, CommState::RECALLING);
275  break;
276  default:
277  ROS_ERROR_NAMED("actionlib",
278  "BUG: Got an unknown goal status from the ActionServer. status = %u",
279  goal_status->status);
280  break;
281  }
282  break;
283  }
284  case CommState::ACTIVE:
285  {
286  switch (goal_status->status) {
287  case actionlib_msgs::GoalStatus::PENDING:
288  ROS_ERROR_NAMED("actionlib", "Invalid transition from ACTIVE to PENDING"); break;
289  case actionlib_msgs::GoalStatus::ACTIVE:
290  break;
291  case actionlib_msgs::GoalStatus::REJECTED:
292  ROS_ERROR_NAMED("actionlib", "Invalid transition from ACTIVE to REJECTED"); break;
293  case actionlib_msgs::GoalStatus::RECALLING:
294  ROS_ERROR_NAMED("actionlib", "Invalid transition from ACTIVE to RECALLING"); break;
295  case actionlib_msgs::GoalStatus::RECALLED:
296  ROS_ERROR_NAMED("actionlib", "Invalid transition from ACTIVE to RECALLED"); break;
297  case actionlib_msgs::GoalStatus::PREEMPTED:
298  transitionToState(gh, CommState::PREEMPTING);
299  transitionToState(gh, CommState::WAITING_FOR_RESULT);
300  break;
301  case actionlib_msgs::GoalStatus::SUCCEEDED:
302  case actionlib_msgs::GoalStatus::ABORTED:
303  transitionToState(gh, CommState::WAITING_FOR_RESULT); break;
304  case actionlib_msgs::GoalStatus::PREEMPTING:
305  transitionToState(gh, CommState::PREEMPTING); break;
306  default:
307  ROS_ERROR_NAMED("actionlib",
308  "BUG: Got an unknown goal status from the ActionServer. status = %u",
309  goal_status->status);
310  break;
311  }
312  break;
313  }
315  {
316  switch (goal_status->status) {
317  case actionlib_msgs::GoalStatus::PENDING:
318  ROS_ERROR_NAMED("actionlib", "Invalid Transition from WAITING_FOR_RESULT to PENDING");
319  break;
320  case actionlib_msgs::GoalStatus::PREEMPTING:
321  ROS_ERROR_NAMED("actionlib", "Invalid Transition from WAITING_FOR_RESULT to PREEMPTING");
322  break;
323  case actionlib_msgs::GoalStatus::RECALLING:
324  ROS_ERROR_NAMED("actionlib", "Invalid Transition from WAITING_FOR_RESULT to RECALLING");
325  break;
326  case actionlib_msgs::GoalStatus::ACTIVE:
327  case actionlib_msgs::GoalStatus::PREEMPTED:
328  case actionlib_msgs::GoalStatus::SUCCEEDED:
329  case actionlib_msgs::GoalStatus::ABORTED:
330  case actionlib_msgs::GoalStatus::REJECTED:
331  case actionlib_msgs::GoalStatus::RECALLED:
332  break;
333  default:
334  ROS_ERROR_NAMED("actionlib",
335  "BUG: Got an unknown state from the ActionServer. status = %u",
336  goal_status->status);
337  break;
338  }
339  break;
340  }
342  {
343  switch (goal_status->status) {
344  case actionlib_msgs::GoalStatus::PENDING:
345  break;
346  case actionlib_msgs::GoalStatus::ACTIVE:
347  break;
348  case actionlib_msgs::GoalStatus::SUCCEEDED:
349  case actionlib_msgs::GoalStatus::ABORTED:
350  case actionlib_msgs::GoalStatus::PREEMPTED:
351  transitionToState(gh, CommState::PREEMPTING);
352  transitionToState(gh, CommState::WAITING_FOR_RESULT);
353  break;
354  case actionlib_msgs::GoalStatus::RECALLED:
355  transitionToState(gh, CommState::RECALLING);
356  transitionToState(gh, CommState::WAITING_FOR_RESULT);
357  break;
358  case actionlib_msgs::GoalStatus::REJECTED:
359  transitionToState(gh, CommState::WAITING_FOR_RESULT); break;
360  case actionlib_msgs::GoalStatus::PREEMPTING:
361  transitionToState(gh, CommState::PREEMPTING); break;
362  case actionlib_msgs::GoalStatus::RECALLING:
363  transitionToState(gh, CommState::RECALLING); break;
364  default:
365  ROS_ERROR_NAMED("actionlib",
366  "BUG: Got an unknown state from the ActionServer. status = %u",
367  goal_status->status);
368  break;
369  }
370  break;
371  }
373  {
374  switch (goal_status->status) {
375  case actionlib_msgs::GoalStatus::PENDING:
376  ROS_ERROR_NAMED("actionlib", "Invalid Transition from RECALLING to PENDING"); break;
377  case actionlib_msgs::GoalStatus::ACTIVE:
378  ROS_ERROR_NAMED("actionlib", "Invalid Transition from RECALLING to ACTIVE"); break;
379  case actionlib_msgs::GoalStatus::SUCCEEDED:
380  case actionlib_msgs::GoalStatus::ABORTED:
381  case actionlib_msgs::GoalStatus::PREEMPTED:
382  transitionToState(gh, CommState::PREEMPTING);
383  transitionToState(gh, CommState::WAITING_FOR_RESULT);
384  break;
385  case actionlib_msgs::GoalStatus::RECALLED:
386  transitionToState(gh, CommState::WAITING_FOR_RESULT);
387  break;
388  case actionlib_msgs::GoalStatus::REJECTED:
389  transitionToState(gh, CommState::WAITING_FOR_RESULT); break;
390  case actionlib_msgs::GoalStatus::PREEMPTING:
391  transitionToState(gh, CommState::PREEMPTING); break;
392  case actionlib_msgs::GoalStatus::RECALLING:
393  break;
394  default:
395  ROS_ERROR_NAMED("actionlib",
396  "BUG: Got an unknown state from the ActionServer. status = %u",
397  goal_status->status);
398  break;
399  }
400  break;
401  }
403  {
404  switch (goal_status->status) {
405  case actionlib_msgs::GoalStatus::PENDING:
406  ROS_ERROR_NAMED("actionlib", "Invalid Transition from PREEMPTING to PENDING"); break;
407  case actionlib_msgs::GoalStatus::ACTIVE:
408  ROS_ERROR_NAMED("actionlib", "Invalid Transition from PREEMPTING to ACTIVE"); break;
409  case actionlib_msgs::GoalStatus::REJECTED:
410  ROS_ERROR_NAMED("actionlib", "Invalid Transition from PREEMPTING to REJECTED"); break;
411  case actionlib_msgs::GoalStatus::RECALLING:
412  ROS_ERROR_NAMED("actionlib", "Invalid Transition from PREEMPTING to RECALLING"); break;
413  case actionlib_msgs::GoalStatus::RECALLED:
414  ROS_ERROR_NAMED("actionlib", "Invalid Transition from PREEMPTING to RECALLED"); break;
415  break;
416  case actionlib_msgs::GoalStatus::PREEMPTED:
417  case actionlib_msgs::GoalStatus::SUCCEEDED:
418  case actionlib_msgs::GoalStatus::ABORTED:
419  transitionToState(gh, CommState::WAITING_FOR_RESULT); break;
420  case actionlib_msgs::GoalStatus::PREEMPTING:
421  break;
422  default:
423  ROS_ERROR_NAMED("actionlib",
424  "BUG: Got an unknown state from the ActionServer. status = %u",
425  goal_status->status);
426  break;
427  }
428  break;
429  }
430  case CommState::DONE:
431  {
432  switch (goal_status->status) {
433  case actionlib_msgs::GoalStatus::PENDING:
434  ROS_ERROR_NAMED("actionlib", "Invalid Transition from DONE to PENDING"); break;
435  case actionlib_msgs::GoalStatus::ACTIVE:
436  ROS_ERROR_NAMED("actionlib", "Invalid Transition from DONE to ACTIVE"); break;
437  case actionlib_msgs::GoalStatus::RECALLING:
438  ROS_ERROR_NAMED("actionlib", "Invalid Transition from DONE to RECALLING"); break;
439  case actionlib_msgs::GoalStatus::PREEMPTING:
440  ROS_ERROR_NAMED("actionlib", "Invalid Transition from DONE to PREEMPTING"); break;
441  case actionlib_msgs::GoalStatus::PREEMPTED:
442  case actionlib_msgs::GoalStatus::SUCCEEDED:
443  case actionlib_msgs::GoalStatus::ABORTED:
444  case actionlib_msgs::GoalStatus::RECALLED:
445  case actionlib_msgs::GoalStatus::REJECTED:
446  break;
447  default:
448  ROS_ERROR_NAMED("actionlib",
449  "BUG: Got an unknown state from the ActionServer. status = %u",
450  goal_status->status);
451  break;
452  }
453  break;
454  }
455  default:
456  ROS_ERROR_NAMED("actionlib", "In a funny comm state: %u", state_.state_);
457  break;
458  }
459 }
460 
461 
462 template<class ActionSpec>
463 void CommStateMachine<ActionSpec>::processLost(GoalHandleT & gh)
464 {
465  ROS_WARN_NAMED("actionlib", "Transitioning goal to LOST");
466  latest_goal_status_.status = actionlib_msgs::GoalStatus::LOST;
467  transitionToState(gh, CommState::DONE);
468 }
469 
470 template<class ActionSpec>
472  const CommState::StateEnum & next_state)
473 {
474  transitionToState(gh, CommState(next_state));
475 }
476 
477 template<class ActionSpec>
478 void CommStateMachine<ActionSpec>::transitionToState(GoalHandleT & gh, const CommState & next_state)
479 {
480  ROS_DEBUG_NAMED("actionlib", "Trying to transition to %s", next_state.toString().c_str());
481  setCommState(next_state);
482  if (transition_cb_) {
483  transition_cb_(gh);
484  }
485 }
486 
487 } // namespace actionlib
488 
489 #endif // ACTIONLIB__CLIENT__COMM_STATE_MACHINE_IMP_H_
actionlib::CommStateMachine::getCommState
CommState getCommState() const
Definition: comm_state_machine_imp.h:101
actionlib::CommStateMachine::getActionGoal
ActionGoalConstPtr getActionGoal() const
Definition: comm_state_machine_imp.h:95
actionlib::CommStateMachine::updateFeedback
void updateFeedback(GoalHandleT &gh, const ActionFeedbackConstPtr &action_feedback)
Definition: comm_state_machine_imp.h:151
actionlib::CommState::StateEnum
StateEnum
Defines the various states the Communication State Machine can be in.
Definition: comm_state.h:115
actionlib::CommStateMachine::transitionToState
void transitionToState(GoalHandleT &gh, const CommState::StateEnum &next_state)
Definition: comm_state_machine_imp.h:503
actionlib::CommState::RECALLING
@ RECALLING
Definition: comm_state.h:154
actionlib::CommStateMachine::updateResult
void updateResult(GoalHandleT &gh, const ActionResultConstPtr &action_result)
Definition: comm_state_machine_imp.h:167
actionlib::CommState::WAITING_FOR_GOAL_ACK
@ WAITING_FOR_GOAL_ACK
Definition: comm_state.h:149
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
actionlib::CommStateMachine::setCommState
void setCommState(const CommState &state)
Change the state, as well as print out ROS_DEBUG info.
Definition: comm_state_machine_imp.h:131
console.h
actionlib::CommState
Thin wrapper around an enum in order to help interpret the state of the communication state machine.
Definition: comm_state.h:79
actionlib::CommState::toString
std::string toString() const
Definition: comm_state.h:150
actionlib::CommStateMachine::findGoalStatus
const actionlib_msgs::GoalStatus * findGoalStatus(const std::vector< actionlib_msgs::GoalStatus > &status_vec) const
Definition: comm_state_machine_imp.h:139
ROS_DEBUG_NAMED
#define ROS_DEBUG_NAMED(name,...)
actionlib::CommState::WAITING_FOR_CANCEL_ACK
@ WAITING_FOR_CANCEL_ACK
Definition: comm_state.h:153
actionlib::CommStateMachine::updateStatus
void updateStatus(GoalHandleT &gh, const actionlib_msgs::GoalStatusArrayConstPtr &status_array)
Definition: comm_state_machine_imp.h:201
actionlib::CommStateMachine
Definition: client_helpers.h:99
actionlib::CommState::ACTIVE
@ ACTIVE
Definition: comm_state.h:151
setup.d
d
Definition: setup.py:6
actionlib::CommStateMachine::CommStateMachine
CommStateMachine()
actionlib::CommState::PENDING
@ PENDING
Definition: comm_state.h:150
actionlib::CommState::WAITING_FOR_RESULT
@ WAITING_FOR_RESULT
Definition: comm_state.h:152
actionlib::CommState::PREEMPTING
@ PREEMPTING
Definition: comm_state.h:155
actionlib::CommStateMachine::getGoalStatus
actionlib_msgs::GoalStatus getGoalStatus() const
Definition: comm_state_machine_imp.h:107
actionlib::CommState::DONE
@ DONE
Definition: comm_state.h:156
actionlib
Definition: action_definition.h:40
ROS_WARN_NAMED
#define ROS_WARN_NAMED(name,...)
actionlib::ClientGoalHandle
Client side handle to monitor goal progress.
Definition: client_helpers.h:96
actionlib::EnclosureDeleter
Definition: enclosure_deleter.h:92
actionlib::CommStateMachine::processLost
void processLost(GoalHandleT &gh)
Definition: comm_state_machine_imp.h:495
actionlib::CommStateMachine::getResult
ResultConstPtr getResult() const
Definition: comm_state_machine_imp.h:113


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep, Mikael Arguedas
autogenerated on Fri May 19 2023 02:36:55