abstract_planner_execution.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
3  *
4  * Redistribution and use in source and binary forms, with or without
5  * modification, are permitted provided that the following conditions
6  * are met:
7  *
8  * 1. Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  *
11  * 2. Redistributions in binary form must reproduce the above
12  * copyright notice, this list of conditions and the following
13  * disclaimer in the documentation and/or other materials provided
14  * with the distribution.
15  *
16  * 3. Neither the name of the copyright holder nor the names of its
17  * contributors may be used to endorse or promote products derived
18  * from this software without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  *
33  * abstract_planner_execution.cpp
34  *
35  * authors:
36  * Sebastian Pütz <spuetz@uni-osnabrueck.de>
37  * Jorge Santos Simón <santos@magazino.eu>
38  *
39  */
40 
42 
43 namespace mbf_abstract_nav
44 {
45 
46 
48  const std::string name,
50  const MoveBaseFlexConfig &config) :
52  planner_(planner_ptr), state_(INITIALIZED), planning_(false),
53  has_new_start_(false), has_new_goal_(false)
54  {
55  ros::NodeHandle private_nh("~");
56 
57  // non-dynamically reconfigurable parameters
58  private_nh.param("robot_frame", robot_frame_, std::string("base_footprint"));
59  private_nh.param("map_frame", global_frame_, std::string("map"));
60 
61  // dynamically reconfigurable parameters
62  reconfigure(config);
63  }
64 
66  {
67  }
68 
69 
71  {
72  boost::lock_guard<boost::mutex> guard(plan_mtx_);
73  // copy plan and costs to output
74  // if the planner plugin do not compute costs compute costs by discrete path length
75  if(cost_ == 0 && !plan_.empty())
76  {
77  ROS_DEBUG_STREAM("Compute costs by discrete path length!");
78  double cost = 0;
79 
80  geometry_msgs::PoseStamped prev_pose = plan_.front();
81  for(std::vector<geometry_msgs::PoseStamped>::iterator iter = plan_.begin() + 1; iter != plan_.end(); ++iter)
82  {
83  cost += mbf_utility::distance(prev_pose, *iter);
84  prev_pose = *iter;
85  }
86  return cost;
87  }
88  return cost_;
89  }
90 
91  void AbstractPlannerExecution::reconfigure(const MoveBaseFlexConfig &config)
92  {
93  boost::lock_guard<boost::mutex> guard(configuration_mutex_);
94 
95  max_retries_ = config.planner_max_retries;
96  frequency_ = config.planner_frequency;
97 
98  // Timeout granted to the global planner. We keep calling it up to this time or up to max_retries times
99  // If it doesn't return within time, the navigator will cancel it and abort the corresponding action
100  patience_ = ros::Duration(config.planner_patience);
101  }
102 
103 
105  {
106  boost::lock_guard<boost::mutex> guard(state_mtx_);
107  return state_;
108  }
109 
111  {
112  boost::lock_guard<boost::mutex> guard(state_mtx_);
113  state_ = state;
114  }
115 
116 
118  {
119  boost::lock_guard<boost::mutex> guard(plan_mtx_);
120  return last_valid_plan_time_;
121  }
122 
123 
125  {
127  }
128 
129 
130  std::vector<geometry_msgs::PoseStamped> AbstractPlannerExecution::getPlan()
131  {
132  boost::lock_guard<boost::mutex> guard(plan_mtx_);
133  // copy plan and costs to output
134  return plan_;
135  }
136 
137 
138  void AbstractPlannerExecution::setNewGoal(const geometry_msgs::PoseStamped &goal, double tolerance)
139  {
140  boost::lock_guard<boost::mutex> guard(goal_start_mtx_);
141  goal_ = goal;
142  tolerance_ = tolerance;
143  has_new_goal_ = true;
144  }
145 
146 
147  void AbstractPlannerExecution::setNewStart(const geometry_msgs::PoseStamped &start)
148  {
149  boost::lock_guard<boost::mutex> guard(goal_start_mtx_);
150  start_ = start;
151  has_new_start_ = true;
152  }
153 
154 
155  void AbstractPlannerExecution::setNewStartAndGoal(const geometry_msgs::PoseStamped &start,
156  const geometry_msgs::PoseStamped &goal,
157  double tolerance)
158  {
159  boost::lock_guard<boost::mutex> guard(goal_start_mtx_);
160  start_ = start;
161  goal_ = goal;
162  tolerance_ = tolerance;
163  has_new_start_ = true;
164  has_new_goal_ = true;
165  }
166 
167 
168  bool AbstractPlannerExecution::start(const geometry_msgs::PoseStamped &start,
169  const geometry_msgs::PoseStamped &goal,
170  double tolerance)
171  {
172  if (planning_)
173  {
174  return false;
175  }
176  boost::lock_guard<boost::mutex> guard(planning_mtx_);
177  planning_ = true;
178  start_ = start;
179  goal_ = goal;
180  tolerance_ = tolerance;
181 
182  geometry_msgs::Point s = start.pose.position;
183  geometry_msgs::Point g = goal.pose.position;
184 
185  ROS_DEBUG_STREAM("Start planning from the start pose: (" << s.x << ", " << s.y << ", " << s.z << ")"
186  << " to the goal pose: ("<< g.x << ", " << g.y << ", " << g.z << ")");
187 
189  }
190 
191 
193  {
194  cancel_ = true; // force cancel immediately, as the call to cancel in the planner can take a while
195 
196  // returns false if cancel is not implemented or rejected by the planner (will run until completion)
197  if(!planner_->cancel())
198  {
199  ROS_WARN_STREAM("Cancel planning failed or is not supported by the plugin. "
200  << "Wait until the current planning finished!");
201 
202  return false;
203  }
204  return true;
205  }
206 
207  uint32_t AbstractPlannerExecution::makePlan(const geometry_msgs::PoseStamped &start,
208  const geometry_msgs::PoseStamped &goal,
209  double tolerance,
210  std::vector<geometry_msgs::PoseStamped> &plan,
211  double &cost,
212  std::string &message)
213  {
214  return planner_->makePlan(start, goal, tolerance, plan, cost, message);
215  }
216 
218  {
219  boost::lock_guard<boost::mutex> guard(planning_mtx_);
220  int retries = 0;
221  geometry_msgs::PoseStamped current_start = start_;
222  geometry_msgs::PoseStamped current_goal = goal_;
223  double current_tolerance = tolerance_;
224 
225  bool success = false;
226  bool make_plan = false;
227  bool exceeded = false;
228 
231 
232  try
233  {
234  while (planning_ && ros::ok())
235  {
236  boost::chrono::thread_clock::time_point start_time = boost::chrono::thread_clock::now();
237 
238  // call the planner
239  std::vector<geometry_msgs::PoseStamped> plan;
240  double cost;
241 
242  // lock goal start mutex
243  goal_start_mtx_.lock();
244  if (has_new_start_)
245  {
246  has_new_start_ = false;
247  current_start = start_;
248  ROS_INFO_STREAM("A new start pose is available. Planning with the new start pose!");
249  exceeded = false;
250  geometry_msgs::Point s = start_.pose.position;
251  ROS_INFO_STREAM("New planning start pose: (" << s.x << ", " << s.y << ", " << s.z << ")");
252  }
253  if (has_new_goal_)
254  {
255  has_new_goal_ = false;
256  current_goal = goal_;
257  current_tolerance = tolerance_;
258  ROS_INFO_STREAM("A new goal pose is available. Planning with the new goal pose and the tolerance: "
259  << current_tolerance);
260  exceeded = false;
261  geometry_msgs::Point g = goal_.pose.position;
262  ROS_INFO_STREAM("New goal pose: (" << g.x << ", " << g.y << ", " << g.z << ")");
263  }
264 
265  make_plan = !(success || exceeded) || has_new_start_ || has_new_goal_;
266 
267  // unlock goal
268  goal_start_mtx_.unlock();
270  if (make_plan)
271  {
272  outcome_ = makePlan(current_start, current_goal, current_tolerance, plan, cost, message_);
273  success = outcome_ < 10;
274 
275  boost::lock_guard<boost::mutex> guard(configuration_mutex_);
276 
277  if (cancel_ && !isPatienceExceeded())
278  {
280  ROS_INFO_STREAM("The global planner has been canceled!"); // but not due to patience exceeded
281  planning_ = false;
283  }
284  else if (success)
285  {
286  ROS_DEBUG_STREAM("Successfully found a plan.");
287  exceeded = false;
288  planning_ = false;
289 
290  plan_mtx_.lock();
291  plan_ = plan;
292  cost_ = cost;
294  plan_mtx_.unlock();
296 
297  condition_.notify_all(); // notify observer
298  }
299  else if (max_retries_ >= 0 && ++retries > max_retries_)
300  {
301  ROS_INFO_STREAM("Planning reached max retries! (" << max_retries_ << ")");
303  exceeded = true;
304  planning_ = false;
305  condition_.notify_all(); // notify observer
306  }
307  else if (isPatienceExceeded())
308  {
309  // Patience exceeded is handled at two levels: here to stop retrying planning when max_retries is
310  // disabled, and on the navigation server when the planner doesn't return for more that patience seconds.
311  // In the second case, the navigation server has tried to cancel planning (possibly without success, as
312  // old nav_core-based planners do not support canceling), and we add here the fact to the log for info
313  ROS_INFO_STREAM("Planning patience (" << patience_.toSec() << "s) has been exceeded"
314  << (cancel_ ? "; planner canceled!" : ""));
316  exceeded = true;
317  planning_ = false;
318  condition_.notify_all(); // notify observer
319  }
320  else if (max_retries_ == 0 && patience_.isZero())
321  {
322  ROS_INFO_STREAM("Planning could not find a plan!");
323  exceeded = true;
325  condition_.notify_all(); // notify observer
326  planning_ = false;
327  }
328  else
329  {
330  exceeded = false;
331  ROS_DEBUG_STREAM("Planning could not find a plan! Trying again...");
332  }
333  }
334  else if (cancel_)
335  {
336  ROS_INFO_STREAM("The global planner has been canceled!");
338  planning_ = false;
340  }
341  } // while (planning_ && ros::ok())
342  }
343  catch (const boost::thread_interrupted &ex)
344  {
345  // Planner thread interrupted; probably we have exceeded planner patience
346  ROS_WARN_STREAM("Planner thread interrupted!");
347  setState(STOPPED);
348  condition_.notify_all(); // notify observer
349  planning_ = false;
350  }
351  catch (...)
352  {
353  ROS_FATAL_STREAM("Unknown error occurred: " << boost::current_exception_diagnostic_information());
355  }
356  }
357 
358 } /* namespace mbf_abstract_nav */
359 
ros::Time last_valid_plan_time_
the last time a valid plan has been computed.
boost::condition_variable condition_
condition variable to wake up control thread
void notify_all() BOOST_NOEXCEPT
PlanningState getState()
Returns the current internal state.
geometry_msgs::PoseStamped goal_
the current goal pose used for planning
AbstractPlannerExecution(const std::string name, const mbf_abstract_core::AbstractPlanner::Ptr planner_ptr, const MoveBaseFlexConfig &config)
Constructor.
bool has_new_start_
true, if a new start pose has been set, until it is used.
virtual uint32_t makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, double tolerance, std::vector< geometry_msgs::PoseStamped > &plan, double &cost, std::string &message)
calls the planner plugin to make a plan from the start pose to the goal pose with the given tolerance...
geometry_msgs::PoseStamped start_
the current start pose used for planning
double tolerance_
optional goal tolerance, in meters
XmlRpcServer s
bool isPatienceExceeded()
Checks whether the patience was exceeded.
No plan has been found (MAX_RETRIES and PAT_EXCEEDED are 0).
PlanningState state_
current internal state
void setState(PlanningState state)
Sets the internal state, thread communication safe.
boost::mutex configuration_mutex_
dynamic reconfigure mutex for a thread safe communication
virtual void run()
The main run method, a thread will execute this method. It contains the main planner execution loop...
bool planning_
main cycle variable of the execution loop
bool cancel_
flag for canceling controlling
std::string message_
the last received plugin execution message
bool param(const std::string &param_name, T &param_val, const T &default_val) const
Exceeded the maximum number of retries without a valid command.
ros::Time getLastValidPlanTime()
Returns the last time a valid plan was available.
#define ROS_FATAL_STREAM(args)
ros::Time last_call_start_time_
the last call start time, updated each cycle.
ROSCPP_DECL bool ok()
uint32_t outcome_
the last received plugin execution outcome
boost::mutex state_mtx_
mutex to handle safe thread communication for the current state
void setNewStart(const geometry_msgs::PoseStamped &start)
Sets a new start pose for the planner execution.
virtual bool cancel()
Cancel the planner execution. This calls the cancel method of the planner plugin. This could be usefu...
Exceeded the patience time without a valid command.
void setNewGoal(const geometry_msgs::PoseStamped &goal, double tolerance)
Sets a new goal pose for the planner execution.
double frequency_
planning cycle frequency (used only when running full navigation; we store here for grouping paramete...
std::vector< geometry_msgs::PoseStamped > getPlan()
Returns a new plan, if one is available.
boost::mutex plan_mtx_
mutex to handle safe thread communication for the plan and plan-costs
#define ROS_WARN_STREAM(args)
#define ROS_DEBUG_STREAM(args)
std::string global_frame_
the global frame in which the planner needs to plan
boost::mutex goal_start_mtx_
mutex to handle safe thread communication for the goal and start pose.
double distance(const geometry_msgs::PoseStamped &pose1, const geometry_msgs::PoseStamped &pose2)
std::vector< geometry_msgs::PoseStamped > plan_
current global plan
#define ROS_INFO_STREAM(args)
void setNewStartAndGoal(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, double tolerance)
Sets a new star and goal pose for the planner execution.
bool has_new_goal_
true, if a new goal pose has been set, until it is used.
static Time now()
mbf_abstract_core::AbstractPlanner::Ptr planner_
the local planer to calculate the velocity command
boost::mutex planning_mtx_
mutex to handle safe thread communication for the planning_ flag.
void reconfigure(const MoveBaseFlexConfig &config)
Is called by the server thread to reconfigure the controller execution, if a user uses dynamic reconf...
std::string robot_frame_
robot frame used for computing the current robot pose
ros::Duration patience_
planning patience duration time


mbf_abstract_nav
Author(s): Sebastian Pütz
autogenerated on Sat Oct 12 2019 04:02:25