goal_manager_imp.h
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* This file has the template implementation for GoalHandle. It should be included with the
36  * class definition.
37  */
38 
39 #ifndef ACTIONLIB__CLIENT__GOAL_MANAGER_IMP_H_
40 #define ACTIONLIB__CLIENT__GOAL_MANAGER_IMP_H_
41 
42 #include "ros/ros.h"
43 
44 namespace actionlib
45 {
46 
47 template<class ActionSpec>
49 {
50  send_goal_func_ = send_goal_func;
51 }
52 
53 template<class ActionSpec>
55 {
56  cancel_func_ = cancel_func;
57 }
58 
59 
60 template<class ActionSpec>
62  TransitionCallback transition_cb,
63  FeedbackCallback feedback_cb)
64 {
65  ActionGoalPtr action_goal(new ActionGoal);
66  action_goal->header.stamp = ros::Time::now();
67  action_goal->goal_id = id_generator_.generateID();
68  action_goal->goal = goal;
69 
70  typedef CommStateMachine<ActionSpec> CommStateMachineT;
71  boost::shared_ptr<CommStateMachineT> comm_state_machine(new CommStateMachineT(action_goal,
72  transition_cb,
73  feedback_cb));
74 
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, _1), guard_);
78 
79  if (send_goal_func_) {
80  send_goal_func_(action_goal);
81  } else {
82  ROS_WARN_NAMED("actionlib",
83  "Possible coding error: send_goal_func_ set to NULL. Not going to send goal");
84  }
85 
86  return GoalHandleT(this, list_handle, guard_);
87 }
88 
89 template<class ActionSpec>
90 void GoalManager<ActionSpec>::listElemDeleter(typename ManagedListT::iterator it)
91 {
92  assert(guard_);
93  if (!guard_)
94  {
95  ROS_ERROR_NAMED("actionlib", "Goal manager deleter should not see invalid guards");
96  return;
97  }
98  DestructionGuard::ScopedProtector protector(*guard_);
99  if (!protector.isProtected()) {
100  ROS_ERROR_NAMED("actionlib",
101  "This action client associated with the goal handle has already been destructed. Not going to try delete the CommStateMachine associated with this goal");
102  return;
103  }
104 
105  ROS_DEBUG_NAMED("actionlib", "About to erase CommStateMachine");
106  boost::recursive_mutex::scoped_lock lock(list_mutex_);
107  list_.erase(it);
108  ROS_DEBUG_NAMED("actionlib", "Done erasing CommStateMachine");
109 }
110 
111 template<class ActionSpec>
113  const actionlib_msgs::GoalStatusArrayConstPtr & status_array)
114 {
115  boost::recursive_mutex::scoped_lock lock(list_mutex_);
116  typename ManagedListT::iterator it = list_.begin();
117 
118  while (it != list_.end()) {
119  GoalHandleT gh(this, it.createHandle(), guard_);
120  (*it)->updateStatus(gh, status_array);
121  ++it;
122  }
123 }
124 
125 template<class ActionSpec>
126 void GoalManager<ActionSpec>::updateFeedbacks(const ActionFeedbackConstPtr & action_feedback)
127 {
128  boost::recursive_mutex::scoped_lock lock(list_mutex_);
129  typename ManagedListT::iterator it = list_.begin();
130 
131  while (it != list_.end()) {
132  GoalHandleT gh(this, it.createHandle(), guard_);
133  (*it)->updateFeedback(gh, action_feedback);
134  ++it;
135  }
136 }
137 
138 template<class ActionSpec>
139 void GoalManager<ActionSpec>::updateResults(const ActionResultConstPtr & action_result)
140 {
141  boost::recursive_mutex::scoped_lock lock(list_mutex_);
142  typename ManagedListT::iterator it = list_.begin();
143 
144  while (it != list_.end()) {
145  GoalHandleT gh(this, it.createHandle(), guard_);
146  (*it)->updateResult(gh, action_result);
147  ++it;
148  }
149 }
150 
151 
152 } // namespace actionlib
153 
154 #endif // ACTIONLIB__CLIENT__GOAL_MANAGER_IMP_H_
boost::function< void(const actionlib_msgs::GoalID &)> CancelFunc
boost::function< void(GoalHandleT, const FeedbackConstPtr &)> FeedbackCallback
#define ROS_WARN_NAMED(name,...)
boost::function< void(GoalHandleT)> TransitionCallback
void updateFeedbacks(const ActionFeedbackConstPtr &action_feedback)
void listElemDeleter(typename ManagedListT::iterator it)
void updateResults(const ActionResultConstPtr &action_result)
bool isProtected()
Checks if the ScopedProtector successfully protected the DestructionGuard.
#define ROS_DEBUG_NAMED(name,...)
boost::function< void(const ActionGoalConstPtr)> SendGoalFunc
GoalHandleT initGoal(const Goal &goal, TransitionCallback transition_cb=TransitionCallback(), FeedbackCallback feedback_cb=FeedbackCallback())
void registerSendGoalFunc(SendGoalFunc send_goal_func)
#define ROS_ERROR_NAMED(name,...)
static Time now()
void updateStatuses(const actionlib_msgs::GoalStatusArrayConstPtr &status_array)
Client side handle to monitor goal progress.
void registerCancelFunc(CancelFunc cancel_func)
Protects a DestructionGuard until this object goes out of scope.


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep, Mikael Arguedas
autogenerated on Mon Aug 24 2020 03:40:47