queued_action_server.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <chrono>
4 #include <condition_variable>
5 #include <memory>
6 #include <mutex>
7 #include <queue>
8 #include <string>
9 #include <thread>
10 
13 #include <ros/ros.h>
14 
15 #include "ensenso_camera/helper.h"
16 
23 template <class ActionSpec>
25 {
26 public:
27  ACTION_DEFINITION(ActionSpec)
28  using GoalHandle = typename actionlib::ActionServer<ActionSpec>::GoalHandle;
29 
30  using ExecuteCallback = boost::function<void(GoalConstPtr const&)>;
31 
32 public:
33  QueuedActionServer(ros::NodeHandle nodeHandle, std::string const& name, ExecuteCallback callback,
34  bool autoStart = false)
35  : nodeHandle(nodeHandle), callback(callback)
36  {
37  actionServer = make_unique<actionlib::ActionServer<ActionSpec>>(nodeHandle, name, false);
38  actionServer->registerGoalCallback(boost::bind(&QueuedActionServer::onGoalReceived, this, _1));
39  actionServer->registerCancelCallback(boost::bind(&QueuedActionServer::onCancelReceived, this, _1));
40 
41  if (autoStart)
42  {
43  start();
44  }
45  }
46 
48  {
49  shutdown();
50  }
51 
52  void start()
53  {
54  actionServer->start();
55  loopThread = std::thread([this] { loop(); }); // NOLINT
56  }
57 
58  void shutdown()
59  {
60  if (loopThread.joinable())
61  {
62  shutdownRequested = true;
63  loopThread.join();
64  }
65  }
66 
68  {
69  return preemptRequested;
70  }
71 
72  void setSucceeded(Result const& result = Result(), std::string const& text = "")
73  {
74  std::lock_guard<std::mutex> lock(mutex);
75 
76  ROS_DEBUG_NAMED("actionlib", "Setting the current goal as succeeded.");
77  currentGoal.setSucceeded(result, text);
78  }
79 
80  void setAborted(Result const& result = Result(), std::string const& text = "")
81  {
82  std::lock_guard<std::mutex> lock(mutex);
83 
84  ROS_DEBUG_NAMED("actionlib", "Setting the current goal as aborted.");
85  currentGoal.setAborted(result, text);
86  }
87 
88  void setPreempted(Result const& result = Result(), std::string const& text = "")
89  {
90  std::lock_guard<std::mutex> lock(mutex);
91 
92  ROS_DEBUG_NAMED("actionlib", "Setting the current goal as canceled.");
93  currentGoal.setCanceled(result, text);
94  }
95 
96  void publishFeedback(FeedbackConstPtr const& feedback)
97  {
98  currentGoal.publishFeedback(*feedback);
99  }
100 
101  void publishFeedback(Feedback const& feedback)
102  {
103  currentGoal.publishFeedback(feedback);
104  }
105 
106 private:
108  {
109  ROS_DEBUG_NAMED("actionlib", "Received a new goal.");
110  std::lock_guard<std::mutex> lock(mutex);
111 
112  goalQueue.push(goal);
113  loopCondition.notify_one();
114  }
115 
117  {
118  ROS_DEBUG_NAMED("actionlib", "Received a cancel request.");
119  std::lock_guard<std::mutex> lock(mutex);
120 
121  if (goal == currentGoal)
122  {
123  // The goal is already being executed. We set the preemption flag
124  // and the user is responsible for polling it and canceling his
125  // action handler.
126  preemptRequested = true;
127  }
128  else
129  {
130  // The goal is in the queue. We cancel it and ignore it later, when
131  // we extract new goals from the queue.
132  goal.setCanceled(Result(), "Goal was canceled by the user.");
133  }
134  }
135 
136  void loop()
137  {
138  std::unique_lock<std::mutex> lock(mutex);
139  while (nodeHandle.ok() && !shutdownRequested)
140  {
141  if (!goalQueue.empty())
142  {
143  currentGoal = goalQueue.front();
144  goalQueue.pop();
145 
146  if (!currentGoal.getGoal())
147  {
148  continue;
149  }
150  else if (currentGoal.getGoalStatus().status == actionlib_msgs::GoalStatus::RECALLING)
151  {
152  currentGoal.setCanceled(Result(), "Goal was canceled by the user.");
153  }
154  else if (currentGoal.getGoalStatus().status != actionlib_msgs::GoalStatus::PENDING)
155  {
156  // The goal might have already been canceled by the cancel
157  // callback above.
158  continue;
159  }
160  else
161  {
162  ROS_DEBUG_NAMED("actionlib", "Accepting a new goal.");
163  currentGoal.setAccepted();
164 
165  preemptRequested = false;
166 
167  lock.unlock();
168  callback(currentGoal.getGoal());
169  lock.lock();
170 
171  if (currentGoal.getGoalStatus().status == actionlib_msgs::GoalStatus::ACTIVE ||
172  currentGoal.getGoalStatus().status == actionlib_msgs::GoalStatus::PREEMPTING)
173  {
174  ROS_WARN_NAMED("actionlib", "Your action handler did not set the "
175  "goal to a terminal state. Aborting it "
176  "for now.");
177  setAborted(Result(), "Aborted, because the user did not set the "
178  "goal to a terminal state.");
179  }
180  }
181  }
182 
183  loopCondition.wait_for(lock, std::chrono::milliseconds(100));
184  }
185  }
186 
187 private:
189  std::unique_ptr<actionlib::ActionServer<ActionSpec>> actionServer;
190 
191  std::thread loopThread;
192 
193  std::mutex mutex;
194  std::condition_variable loopCondition;
195 
197 
199  std::queue<GoalHandle> goalQueue;
200 
201  // Whether the user canceled the goal that is currently being executed.
202  bool preemptRequested = false;
203 
204  volatile bool shutdownRequested = false;
205 };
void setPreempted(Result const &result=Result(), std::string const &text="")
typename actionlib::ActionServer< ActionSpec >::GoalHandle GoalHandle
volatile bool shutdownRequested
#define ROS_WARN_NAMED(name,...)
void onCancelReceived(GoalHandle goal)
ExecuteCallback callback
void setAborted(Result const &result=Result(), std::string const &text="")
void publishFeedback(Feedback const &feedback)
boost::function< void(GoalConstPtr const &)> ExecuteCallback
#define ROS_DEBUG_NAMED(name,...)
std::queue< GoalHandle > goalQueue
std::unique_ptr< actionlib::ActionServer< ActionSpec > > actionServer
std::condition_variable loopCondition
#define ACTION_DEFINITION(ActionSpec)
ros::NodeHandle nodeHandle
void onGoalReceived(GoalHandle goal)
bool ok() const
void setSucceeded(Result const &result=Result(), std::string const &text="")
void publishFeedback(FeedbackConstPtr const &feedback)


ensenso_camera
Author(s): Ensenso
autogenerated on Thu May 16 2019 02:44:23