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
00036
00037
00038
00039 #ifndef ACTIONLIB__CLIENT__GOAL_MANAGER_IMP_H_
00040 #define ACTIONLIB__CLIENT__GOAL_MANAGER_IMP_H_
00041
00042 namespace actionlib
00043 {
00044
00045 template<class ActionSpec>
00046 void GoalManager<ActionSpec>::registerSendGoalFunc(SendGoalFunc send_goal_func)
00047 {
00048 send_goal_func_ = send_goal_func;
00049 }
00050
00051 template<class ActionSpec>
00052 void GoalManager<ActionSpec>::registerCancelFunc(CancelFunc cancel_func)
00053 {
00054 cancel_func_ = cancel_func;
00055 }
00056
00057
00058 template<class ActionSpec>
00059 ClientGoalHandle<ActionSpec> GoalManager<ActionSpec>::initGoal(const Goal & goal,
00060 TransitionCallback transition_cb,
00061 FeedbackCallback feedback_cb)
00062 {
00063 ActionGoalPtr action_goal(new ActionGoal);
00064 action_goal->header.stamp = ros::Time::now();
00065 action_goal->goal_id = id_generator_.generateID();
00066 action_goal->goal = goal;
00067
00068 typedef CommStateMachine<ActionSpec> CommStateMachineT;
00069 boost::shared_ptr<CommStateMachineT> comm_state_machine(new CommStateMachineT(action_goal,
00070 transition_cb,
00071 feedback_cb));
00072
00073 boost::recursive_mutex::scoped_lock lock(list_mutex_);
00074 typename ManagedListT::Handle list_handle =
00075 list_.add(comm_state_machine, boost::bind(&GoalManagerT::listElemDeleter, this, _1), guard_);
00076
00077 if (send_goal_func_) {
00078 send_goal_func_(action_goal);
00079 } else {
00080 ROS_WARN_NAMED("actionlib",
00081 "Possible coding error: send_goal_func_ set to NULL. Not going to send goal");
00082 }
00083
00084 return GoalHandleT(this, list_handle, guard_);
00085 }
00086
00087 template<class ActionSpec>
00088 void GoalManager<ActionSpec>::listElemDeleter(typename ManagedListT::iterator it)
00089 {
00090 assert(guard_);
00091 if (!guard_)
00092 {
00093 ROS_ERROR_NAMED("actionlib", "Goal manager deleter should not see invalid guards");
00094 return;
00095 }
00096 DestructionGuard::ScopedProtector protector(*guard_);
00097 if (!protector.isProtected()) {
00098 ROS_ERROR_NAMED("actionlib",
00099 "This action client associated with the goal handle has already been destructed. Not going to try delete the CommStateMachine associated with this goal");
00100 return;
00101 }
00102
00103 ROS_DEBUG_NAMED("actionlib", "About to erase CommStateMachine");
00104 boost::recursive_mutex::scoped_lock lock(list_mutex_);
00105 list_.erase(it);
00106 ROS_DEBUG_NAMED("actionlib", "Done erasing CommStateMachine");
00107 }
00108
00109 template<class ActionSpec>
00110 void GoalManager<ActionSpec>::updateStatuses(
00111 const actionlib_msgs::GoalStatusArrayConstPtr & status_array)
00112 {
00113 boost::recursive_mutex::scoped_lock lock(list_mutex_);
00114 typename ManagedListT::iterator it = list_.begin();
00115
00116 while (it != list_.end()) {
00117 GoalHandleT gh(this, it.createHandle(), guard_);
00118 (*it)->updateStatus(gh, status_array);
00119 ++it;
00120 }
00121 }
00122
00123 template<class ActionSpec>
00124 void GoalManager<ActionSpec>::updateFeedbacks(const ActionFeedbackConstPtr & action_feedback)
00125 {
00126 boost::recursive_mutex::scoped_lock lock(list_mutex_);
00127 typename ManagedListT::iterator it = list_.begin();
00128
00129 while (it != list_.end()) {
00130 GoalHandleT gh(this, it.createHandle(), guard_);
00131 (*it)->updateFeedback(gh, action_feedback);
00132 ++it;
00133 }
00134 }
00135
00136 template<class ActionSpec>
00137 void GoalManager<ActionSpec>::updateResults(const ActionResultConstPtr & action_result)
00138 {
00139 boost::recursive_mutex::scoped_lock lock(list_mutex_);
00140 typename ManagedListT::iterator it = list_.begin();
00141
00142 while (it != list_.end()) {
00143 GoalHandleT gh(this, it.createHandle(), guard_);
00144 (*it)->updateResult(gh, action_result);
00145 ++it;
00146 }
00147 }
00148
00149
00150 }
00151
00152 #endif // ACTIONLIB__CLIENT__GOAL_MANAGER_IMP_H_