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__CLIENT__CLIENT_HELPERS_H_
00036 #define ACTIONLIB__CLIENT__CLIENT_HELPERS_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 #include <vector>
00043
00044
00045 #include "actionlib/action_definition.h"
00046
00047 #include "actionlib/managed_list.h"
00048 #include "actionlib/enclosure_deleter.h"
00049 #include "actionlib/goal_id_generator.h"
00050
00051 #include "actionlib/client/comm_state.h"
00052 #include "actionlib/client/terminal_state.h"
00053
00054 #include "actionlib/destruction_guard.h"
00055
00056
00057 #include "actionlib_msgs/GoalID.h"
00058 #include "actionlib_msgs/GoalStatusArray.h"
00059
00060 namespace actionlib
00061 {
00062
00063 template<class ActionSpec>
00064 class ClientGoalHandle;
00065
00066 template<class ActionSpec>
00067 class CommStateMachine;
00068
00069 template<class ActionSpec>
00070 class GoalManager
00071 {
00072 public:
00073 ACTION_DEFINITION(ActionSpec);
00074 typedef GoalManager<ActionSpec> GoalManagerT;
00075 typedef ClientGoalHandle<ActionSpec> GoalHandleT;
00076 typedef boost::function<void (GoalHandleT)> TransitionCallback;
00077 typedef boost::function<void (GoalHandleT, const FeedbackConstPtr &)> FeedbackCallback;
00078 typedef boost::function<void (const ActionGoalConstPtr)> SendGoalFunc;
00079 typedef boost::function<void (const actionlib_msgs::GoalID &)> CancelFunc;
00080
00081 GoalManager(const boost::shared_ptr<DestructionGuard> & guard)
00082 : guard_(guard) {}
00083
00084 void registerSendGoalFunc(SendGoalFunc send_goal_func);
00085 void registerCancelFunc(CancelFunc cancel_func);
00086
00087 GoalHandleT initGoal(const Goal & goal,
00088 TransitionCallback transition_cb = TransitionCallback(),
00089 FeedbackCallback feedback_cb = FeedbackCallback() );
00090
00091 void updateStatuses(const actionlib_msgs::GoalStatusArrayConstPtr & status_array);
00092 void updateFeedbacks(const ActionFeedbackConstPtr & action_feedback);
00093 void updateResults(const ActionResultConstPtr & action_result);
00094
00095 friend class ClientGoalHandle<ActionSpec>;
00096
00097
00098 typedef ManagedList<boost::shared_ptr<CommStateMachine<ActionSpec> > > ManagedListT;
00099 ManagedListT list_;
00100
00101 private:
00102 SendGoalFunc send_goal_func_;
00103 CancelFunc cancel_func_;
00104
00105 boost::shared_ptr<DestructionGuard> guard_;
00106
00107 boost::recursive_mutex list_mutex_;
00108
00109 GoalIDGenerator id_generator_;
00110
00111 void listElemDeleter(typename ManagedListT::iterator it);
00112 };
00113
00121 template<class ActionSpec>
00122 class ClientGoalHandle
00123 {
00124 private:
00125 ACTION_DEFINITION(ActionSpec);
00126
00127 public:
00134 ClientGoalHandle();
00135
00136 ~ClientGoalHandle();
00137
00144 void reset();
00145
00152 inline bool isExpired() const;
00153
00161 CommState getCommState() const;
00162
00170 TerminalState getTerminalState() const;
00171
00177 ResultConstPtr getResult() const;
00178
00184 void resend();
00185
00191 void cancel();
00192
00197 bool operator==(const ClientGoalHandle<ActionSpec> & rhs) const;
00198
00202 bool operator!=(const ClientGoalHandle<ActionSpec> & rhs) const;
00203
00204 friend class GoalManager<ActionSpec>;
00205
00206 private:
00207 typedef GoalManager<ActionSpec> GoalManagerT;
00208 typedef ManagedList<boost::shared_ptr<CommStateMachine<ActionSpec> > > ManagedListT;
00209
00210 ClientGoalHandle(GoalManagerT * gm, typename ManagedListT::Handle handle,
00211 const boost::shared_ptr<DestructionGuard> & guard);
00212
00213 GoalManagerT * gm_;
00214 bool active_;
00215
00216 boost::shared_ptr<DestructionGuard> guard_;
00217 typename ManagedListT::Handle list_handle_;
00218 };
00219
00220 template<class ActionSpec>
00221 class CommStateMachine
00222 {
00223 private:
00224
00225 ACTION_DEFINITION(ActionSpec);
00226
00227 public:
00228 typedef boost::function<void (const ClientGoalHandle<ActionSpec> &)> TransitionCallback;
00229 typedef boost::function<void (const ClientGoalHandle<ActionSpec> &,
00230 const FeedbackConstPtr &)> FeedbackCallback;
00231 typedef ClientGoalHandle<ActionSpec> GoalHandleT;
00232
00233 CommStateMachine(const ActionGoalConstPtr & action_goal,
00234 TransitionCallback transition_cb,
00235 FeedbackCallback feedback_cb);
00236
00237 ActionGoalConstPtr getActionGoal() const;
00238 CommState getCommState() const;
00239 actionlib_msgs::GoalStatus getGoalStatus() const;
00240 ResultConstPtr getResult() const;
00241
00242
00243 void updateStatus(GoalHandleT & gh, const actionlib_msgs::GoalStatusArrayConstPtr & status_array);
00244 void updateFeedback(GoalHandleT & gh, const ActionFeedbackConstPtr & action_feedback);
00245 void updateResult(GoalHandleT & gh, const ActionResultConstPtr & action_result);
00246
00247
00248 void transitionToState(GoalHandleT & gh, const CommState::StateEnum & next_state);
00249 void transitionToState(GoalHandleT & gh, const CommState & next_state);
00250 void processLost(GoalHandleT & gh);
00251
00252 private:
00253 CommStateMachine();
00254
00255
00256 CommState state_;
00257 ActionGoalConstPtr action_goal_;
00258 actionlib_msgs::GoalStatus latest_goal_status_;
00259 ActionResultConstPtr latest_result_;
00260
00261
00262 TransitionCallback transition_cb_;
00263 FeedbackCallback feedback_cb_;
00264
00265
00267 void setCommState(const CommState & state);
00268 void setCommState(const CommState::StateEnum & state);
00269 const actionlib_msgs::GoalStatus * findGoalStatus(
00270 const std::vector<actionlib_msgs::GoalStatus> & status_vec) const;
00271 };
00272
00273 }
00274
00275 #include "actionlib/client/goal_manager_imp.h"
00276 #include "actionlib/client/client_goal_handle_imp.h"
00277 #include "actionlib/client/comm_state_machine_imp.h"
00278
00279 #endif // ACTIONLIB__CLIENT__CLIENT_HELPERS_H_