Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #ifndef ACTIONLIB_GOAL_MANAGER_H_
00036 #define ACTIONLIB_GOAL_MANAGER_H_
00037
00038 #include <boost/thread/recursive_mutex.hpp>
00039 #include <boost/interprocess/sync/scoped_lock.hpp>
00040 #include <boost/shared_ptr.hpp>
00041 #include <boost/weak_ptr.hpp>
00042
00043
00044 #include "actionlib/action_definition.h"
00045
00046 #include "actionlib/managed_list.h"
00047 #include "actionlib/enclosure_deleter.h"
00048 #include "actionlib/goal_id_generator.h"
00049
00050 #include "actionlib/client/comm_state.h"
00051 #include "actionlib/client/terminal_state.h"
00052
00053 #include "actionlib/destruction_guard.h"
00054
00055
00056 #include "actionlib_msgs/GoalID.h"
00057 #include "actionlib_msgs/GoalStatusArray.h"
00058
00059 namespace actionlib
00060 {
00061
00062 template <class ActionSpec>
00063 class ClientGoalHandle;
00064
00065 template <class ActionSpec>
00066 class CommStateMachine;
00067
00068 template <class ActionSpec>
00069 class GoalManager
00070 {
00071 public:
00072 ACTION_DEFINITION(ActionSpec);
00073 typedef GoalManager<ActionSpec> GoalManagerT;
00074 typedef ClientGoalHandle<ActionSpec> GoalHandleT;
00075 typedef boost::function<void (GoalHandleT) > TransitionCallback;
00076 typedef boost::function<void (GoalHandleT, const FeedbackConstPtr&) > FeedbackCallback;
00077 typedef boost::function<void (const ActionGoalConstPtr)> SendGoalFunc;
00078 typedef boost::function<void (const actionlib_msgs::GoalID&)> CancelFunc;
00079
00080 GoalManager(const boost::shared_ptr<DestructionGuard>& guard) : guard_(guard) { }
00081
00082 void registerSendGoalFunc(SendGoalFunc send_goal_func);
00083 void registerCancelFunc(CancelFunc cancel_func);
00084
00085 GoalHandleT initGoal( const Goal& goal,
00086 TransitionCallback transition_cb = TransitionCallback(),
00087 FeedbackCallback feedback_cb = FeedbackCallback() );
00088
00089 void updateStatuses(const actionlib_msgs::GoalStatusArrayConstPtr& status_array);
00090 void updateFeedbacks(const ActionFeedbackConstPtr& action_feedback);
00091 void updateResults(const ActionResultConstPtr& action_result);
00092
00093 friend class ClientGoalHandle<ActionSpec>;
00094
00095
00096 typedef ManagedList< boost::shared_ptr<CommStateMachine<ActionSpec> > > ManagedListT;
00097 ManagedListT list_;
00098 private:
00099 SendGoalFunc send_goal_func_ ;
00100 CancelFunc cancel_func_ ;
00101
00102 boost::shared_ptr<DestructionGuard> guard_;
00103
00104 boost::recursive_mutex list_mutex_;
00105
00106 GoalIDGenerator id_generator_;
00107
00108 void listElemDeleter(typename ManagedListT::iterator it);
00109 };
00110
00118 template <class ActionSpec>
00119 class ClientGoalHandle
00120 {
00121 private:
00122 ACTION_DEFINITION(ActionSpec);
00123
00124 public:
00131 ClientGoalHandle();
00132
00133 ~ClientGoalHandle();
00134
00141 void reset();
00142
00149 inline bool isExpired() const;
00150
00158 CommState getCommState();
00159
00167 TerminalState getTerminalState();
00168
00174 ResultConstPtr getResult();
00175
00181 void resend();
00182
00188 void cancel();
00189
00194 bool operator==(const ClientGoalHandle<ActionSpec>& rhs) const;
00195
00199 bool operator!=(const ClientGoalHandle<ActionSpec>& rhs) const;
00200
00201 friend class GoalManager<ActionSpec>;
00202 private:
00203 typedef GoalManager<ActionSpec> GoalManagerT;
00204 typedef ManagedList< boost::shared_ptr<CommStateMachine<ActionSpec> > > ManagedListT;
00205
00206 ClientGoalHandle(GoalManagerT* gm, typename ManagedListT::Handle handle, const boost::shared_ptr<DestructionGuard>& guard);
00207
00208 GoalManagerT* gm_;
00209 bool active_;
00210
00211 boost::shared_ptr<DestructionGuard> guard_;
00212 typename ManagedListT::Handle list_handle_;
00213 };
00214
00215 template <class ActionSpec>
00216 class CommStateMachine
00217 {
00218 private:
00219
00220 ACTION_DEFINITION(ActionSpec);
00221
00222 public:
00223 typedef boost::function<void (const ClientGoalHandle<ActionSpec>&) > TransitionCallback;
00224 typedef boost::function<void (const ClientGoalHandle<ActionSpec>&, const FeedbackConstPtr&) > FeedbackCallback;
00225 typedef ClientGoalHandle<ActionSpec> GoalHandleT;
00226
00227 CommStateMachine(const ActionGoalConstPtr& action_goal,
00228 TransitionCallback transition_callback,
00229 FeedbackCallback feedback_callback);
00230
00231 ActionGoalConstPtr getActionGoal() const;
00232 CommState getCommState() const;
00233 actionlib_msgs::GoalStatus getGoalStatus() const;
00234 ResultConstPtr getResult() const;
00235
00236
00237 void updateStatus(GoalHandleT& gh, const actionlib_msgs::GoalStatusArrayConstPtr& status_array);
00238 void updateFeedback(GoalHandleT& gh, const ActionFeedbackConstPtr& feedback);
00239 void updateResult(GoalHandleT& gh, const ActionResultConstPtr& result);
00240
00241
00242 void transitionToState(GoalHandleT& gh, const CommState::StateEnum& next_state);
00243 void transitionToState(GoalHandleT& gh, const CommState& next_state);
00244 void processLost(GoalHandleT& gh);
00245 private:
00246 CommStateMachine();
00247
00248
00249 CommState state_;
00250 ActionGoalConstPtr action_goal_;
00251 actionlib_msgs::GoalStatus latest_goal_status_;
00252 ActionResultConstPtr latest_result_;
00253
00254
00255 TransitionCallback transition_cb_;
00256 FeedbackCallback feedback_cb_;
00257
00258
00260 void setCommState(const CommState& state);
00261 void setCommState(const CommState::StateEnum& state);
00262 const actionlib_msgs::GoalStatus* findGoalStatus(const std::vector<actionlib_msgs::GoalStatus>& status_vec) const;
00263 };
00264
00265 }
00266
00267 #include "actionlib/client/goal_manager_imp.h"
00268 #include "actionlib/client/client_goal_handle_imp.h"
00269 #include "actionlib/client/comm_state_machine_imp.h"
00270
00271 #endif // ACTIONLIB_GOAL_MANAGER_H_