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 
47  const std::string name,
49  const MoveBaseFlexConfig &config) :
51  planner_(planner_ptr), state_(INITIALIZED), planning_(false),
52  has_new_start_(false), has_new_goal_(false)
53 {
54  ros::NodeHandle private_nh("~");
55 
56  // non-dynamically reconfigurable parameters
57  private_nh.param("robot_frame", robot_frame_, std::string("base_footprint"));
58  private_nh.param("map_frame", global_frame_, std::string("map"));
59 
60  // dynamically reconfigurable parameters
61  reconfigure(config);
62 }
63 
65 {
66 }
67 
68 
70 {
71  boost::lock_guard<boost::mutex> guard(plan_mtx_);
72  // copy plan and costs to output
73  // if the planner plugin do not compute costs compute costs by discrete path length
74  if(cost_ == 0 && !plan_.empty())
75  {
76  ROS_DEBUG_STREAM("Compute costs by discrete path length!");
77  double cost = 0;
78 
79  geometry_msgs::PoseStamped prev_pose = plan_.front();
80  for(std::vector<geometry_msgs::PoseStamped>::iterator iter = plan_.begin() + 1; iter != plan_.end(); ++iter)
81  {
82  cost += mbf_utility::distance(prev_pose, *iter);
83  prev_pose = *iter;
84  }
85  return cost;
86  }
87  return cost_;
88 }
89 
90 void AbstractPlannerExecution::reconfigure(const MoveBaseFlexConfig &config)
91 {
92  boost::lock_guard<boost::mutex> guard(configuration_mutex_);
93 
94  max_retries_ = config.planner_max_retries;
95  frequency_ = config.planner_frequency;
96 
97  // Timeout granted to the global planner. We keep calling it up to this time or up to max_retries times
98  // If it doesn't return within time, the navigator will cancel it and abort the corresponding action
99  patience_ = ros::Duration(config.planner_patience);
100 }
101 
102 
104 {
105  boost::lock_guard<boost::mutex> guard(state_mtx_);
106  return state_;
107 }
108 
110 {
111  boost::lock_guard<boost::mutex> guard(state_mtx_);
112  state_ = state;
113 }
114 
115 
117 {
118  boost::lock_guard<boost::mutex> guard(plan_mtx_);
119  return last_valid_plan_time_;
120 }
121 
122 
124 {
126 }
127 
128 
129 std::vector<geometry_msgs::PoseStamped> AbstractPlannerExecution::getPlan()
130 {
131  boost::lock_guard<boost::mutex> guard(plan_mtx_);
132  // copy plan and costs to output
133  return plan_;
134 }
135 
136 
137 void AbstractPlannerExecution::setNewGoal(const geometry_msgs::PoseStamped &goal, double tolerance)
138 {
139  boost::lock_guard<boost::mutex> guard(goal_start_mtx_);
140  goal_ = goal;
141  tolerance_ = tolerance;
142  has_new_goal_ = true;
143 }
144 
145 
146 void AbstractPlannerExecution::setNewStart(const geometry_msgs::PoseStamped &start)
147 {
148  boost::lock_guard<boost::mutex> guard(goal_start_mtx_);
149  start_ = start;
150  has_new_start_ = true;
151 }
152 
153 
154 void AbstractPlannerExecution::setNewStartAndGoal(const geometry_msgs::PoseStamped &start,
155  const geometry_msgs::PoseStamped &goal,
156  double tolerance)
157 {
158  boost::lock_guard<boost::mutex> guard(goal_start_mtx_);
159  start_ = start;
160  goal_ = goal;
161  tolerance_ = tolerance;
162  has_new_start_ = true;
163  has_new_goal_ = true;
164 }
165 
166 
167 bool AbstractPlannerExecution::start(const geometry_msgs::PoseStamped &start,
168  const geometry_msgs::PoseStamped &goal,
169  double tolerance)
170 {
171  if (planning_)
172  {
173  return false;
174  }
175  boost::lock_guard<boost::mutex> guard(planning_mtx_);
176  planning_ = true;
177  start_ = start;
178  goal_ = goal;
179  tolerance_ = tolerance;
180 
181  geometry_msgs::Point s = start.pose.position;
182  geometry_msgs::Point g = goal.pose.position;
183 
184  ROS_DEBUG_STREAM("Start planning from the start pose: (" << s.x << ", " << s.y << ", " << s.z << ")"
185  << " to the goal pose: ("<< g.x << ", " << g.y << ", " << g.z << ")");
186 
188 }
189 
190 
192 {
193  cancel_ = true; // force cancel immediately, as the call to cancel in the planner can take a while
194 
195  // returns false if cancel is not implemented or rejected by the planner (will run until completion)
196  if (!planner_->cancel())
197  {
198  ROS_WARN_STREAM("Cancel planning failed or is not supported by the plugin. "
199  << "Wait until the current planning finished!");
200 
201  return false;
202  }
203  return true;
204 }
205 
206 uint32_t AbstractPlannerExecution::makePlan(const geometry_msgs::PoseStamped &start,
207  const geometry_msgs::PoseStamped &goal,
208  double tolerance,
209  std::vector<geometry_msgs::PoseStamped> &plan,
210  double &cost,
211  std::string &message)
212 {
213  return planner_->makePlan(start, goal, tolerance, plan, cost, message);
214 }
215 
217 {
218  setState(STARTED);
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 planner \"" << name_ << "\" has been canceled!"); // but not due to patience exceeded
281  planning_ = false;
282  condition_.notify_all();
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;
339  condition_.notify_all();
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  condition_.notify_all();
356  }
357 }
358 
359 } /* namespace mbf_abstract_nav */
360 
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
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.
std::string name_
the plugin name; not the plugin type!
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 Wed May 27 2020 04:03:14