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