actionlib: goal_manager_imp.h Source File

00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* This file has the template implementation for GoalHandle. It should be included with the
00036  * class definition.
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("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("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("About to erase CommStateMachine");
00091   boost::recursive_mutex::scoped_lock lock(list_mutex_);
00092   list_.erase(it);
00093   ROS_DEBUG("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 }