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