39 #ifndef ACTIONLIB__CLIENT__GOAL_MANAGER_IMP_H_
40 #define ACTIONLIB__CLIENT__GOAL_MANAGER_IMP_H_
47 template<
class ActionSpec>
50 send_goal_func_ = send_goal_func;
53 template<
class ActionSpec>
56 cancel_func_ = cancel_func;
60 template<
class ActionSpec>
62 TransitionCallback transition_cb,
63 FeedbackCallback feedback_cb)
65 ActionGoalPtr action_goal(
new ActionGoal);
67 action_goal->goal_id = id_generator_.generateID();
68 action_goal->goal = goal;
70 typedef CommStateMachine<ActionSpec> CommStateMachineT;
75 boost::recursive_mutex::scoped_lock lock(list_mutex_);
76 typename ManagedListT::Handle list_handle =
77 list_.add(comm_state_machine, boost::bind(&GoalManagerT::listElemDeleter,
this, boost::placeholders::_1), guard_);
79 if (send_goal_func_) {
80 send_goal_func_(action_goal);
83 "Possible coding error: send_goal_func_ set to nullptr. Not going to send goal");
89 template<
class ActionSpec>
95 ROS_ERROR_NAMED(
"actionlib",
"Goal manager deleter should not see invalid guards");
99 if (!protector.isProtected()) {
101 "This action client associated with the goal handle has already been destructed. Not going to try delete the CommStateMachine associated with this goal");
106 boost::recursive_mutex::scoped_lock lock(list_mutex_);
111 template<
class ActionSpec>
113 const actionlib_msgs::GoalStatusArrayConstPtr & status_array)
115 boost::recursive_mutex::scoped_lock lock(list_mutex_);
116 typename ManagedListT::iterator it = list_.begin();
118 while (it != list_.end()) {
119 GoalHandleT gh(
this, it.createHandle(), guard_);
120 (*it)->updateStatus(gh, status_array);
125 template<
class ActionSpec>
128 boost::recursive_mutex::scoped_lock lock(list_mutex_);
129 typename ManagedListT::iterator it = list_.begin();
131 while (it != list_.end()) {
132 GoalHandleT gh(
this, it.createHandle(), guard_);
133 (*it)->updateFeedback(gh, action_feedback);
138 template<
class ActionSpec>
141 boost::recursive_mutex::scoped_lock lock(list_mutex_);
142 typename ManagedListT::iterator it = list_.begin();
144 while (it != list_.end()) {
146 (*it)->updateResult(gh, action_result);
154 #endif // ACTIONLIB__CLIENT__GOAL_MANAGER_IMP_H_