move_base_action.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  * move_base_action.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 #include "mbf_abstract_nav/MoveBaseFlexConfig.h"
45 
46 namespace mbf_abstract_nav
47 {
48 
49 MoveBaseAction::MoveBaseAction(const std::string &name,
50  const mbf_utility::RobotInformation &robot_info,
51  const std::vector<std::string> &behaviors)
52  : name_(name), robot_info_(robot_info), private_nh_("~"),
53  action_client_exe_path_(private_nh_, "exe_path"),
54  action_client_get_path_(private_nh_, "get_path"),
55  action_client_recovery_(private_nh_, "recovery"),
56  oscillation_timeout_(0),
57  oscillation_distance_(0),
58  recovery_enabled_(true),
59  behaviors_(behaviors),
60  action_state_(NONE),
61  recovery_trigger_(NONE),
62  replanning_(false),
63  replanning_rate_(1.0)
64 {
65 }
66 
68 {
69 }
70 
72  mbf_abstract_nav::MoveBaseFlexConfig &config, uint32_t level)
73 {
74  if (config.planner_frequency > 0.0)
75  {
76  boost::lock_guard<boost::mutex> guard(replanning_mtx_);
77  if (!replanning_)
78  {
79  replanning_ = true;
80  if (action_state_ == EXE_PATH &&
83  {
84  // exe_path is running and user has enabled replanning
85  ROS_INFO_STREAM_NAMED("move_base", "Planner frequency set to " << config.planner_frequency
86  << ": start replanning, using the \"get_path\" action!");
89  boost::bind(&MoveBaseAction::actionGetPathReplanningDone, this, _1, _2)
90  );
91  }
92  }
93  replanning_rate_ = ros::Rate(config.planner_frequency);
94  }
95  else
96  replanning_ = false;
97  oscillation_timeout_ = ros::Duration(config.oscillation_timeout);
98  oscillation_distance_ = config.oscillation_distance;
99  recovery_enabled_ = config.recovery_enabled;
100 }
101 
103 {
105 
107  {
109  }
110 
112  {
114  }
115 
117  {
119  }
120 }
121 
123 {
125 
126  goal_handle.setAccepted();
127 
128  goal_handle_ = goal_handle;
129 
130  ROS_DEBUG_STREAM_NAMED("move_base", "Start action \"move_base\"");
131 
132  const mbf_msgs::MoveBaseGoal& goal = *goal_handle.getGoal();
133 
134  mbf_msgs::MoveBaseResult move_base_result;
135 
136  get_path_goal_.target_pose = goal.target_pose;
137  get_path_goal_.use_start_pose = false; // use the robot pose
138  get_path_goal_.planner = goal.planner;
139  exe_path_goal_.controller = goal.controller;
140 
141  ros::Duration connection_timeout(1.0);
142 
144 
145  // start recovering with the first behavior, use the recovery behaviors from the action request, if specified,
146  // otherwise, use all loaded behaviors.
147 
148  recovery_behaviors_ = goal.recovery_behaviors.empty() ? behaviors_ : goal.recovery_behaviors;
150 
151  // get the current robot pose only at the beginning, as exe_path will keep updating it as we move
153  {
154  ROS_ERROR_STREAM_NAMED("move_base", "Could not get the current robot pose!");
155  move_base_result.message = "Could not get the current robot pose!";
156  move_base_result.outcome = mbf_msgs::MoveBaseResult::TF_ERROR;
157  goal_handle.setAborted(move_base_result, move_base_result.message);
158  return;
159  }
160  goal_pose_ = goal.target_pose;
161 
162  // wait for server connections
163  if (!action_client_get_path_.waitForServer(connection_timeout) ||
164  !action_client_exe_path_.waitForServer(connection_timeout) ||
165  !action_client_recovery_.waitForServer(connection_timeout))
166  {
167  ROS_ERROR_STREAM_NAMED("move_base", "Could not connect to one or more of move_base_flex actions: "
168  "\"get_path\", \"exe_path\", \"recovery \"!");
169  move_base_result.outcome = mbf_msgs::MoveBaseResult::INTERNAL_ERROR;
170  move_base_result.message = "Could not connect to the move_base_flex actions!";
171  goal_handle.setAborted(move_base_result, move_base_result.message);
172  return;
173  }
174 
175  // call get_path action server to get a first plan
178  boost::bind(&MoveBaseAction::actionGetPathDone, this, _1, _2));
179 }
180 
182 {
183  ROS_DEBUG_STREAM_NAMED("move_base", "The \"exe_path\" action is active.");
184 }
185 
187  const mbf_msgs::ExePathFeedbackConstPtr &feedback)
188 {
189  mbf_msgs::MoveBaseFeedback move_base_feedback;
190  move_base_feedback.outcome = feedback->outcome;
191  move_base_feedback.message = feedback->message;
192  move_base_feedback.angle_to_goal = feedback->angle_to_goal;
193  move_base_feedback.dist_to_goal = feedback->dist_to_goal;
194  move_base_feedback.current_pose = feedback->current_pose;
195  move_base_feedback.last_cmd_vel = feedback->last_cmd_vel;
196  robot_pose_ = feedback->current_pose;
197  goal_handle_.publishFeedback(move_base_feedback);
198 
199  // we create a navigation-level oscillation detection using exe_path action's feedback,
200  // as the later doesn't handle oscillations created by quickly failing repeated plans
201 
202  // if oscillation detection is enabled by oscillation_timeout != 0
204  {
205  // check if oscillating
206  // moved more than the minimum oscillation distance
208  {
211 
213  {
214  ROS_INFO_NAMED("move_base", "Recovered from robot oscillation: restart recovery behaviors");
217  }
218  }
220  {
221  std::stringstream oscillation_msgs;
222  oscillation_msgs << "Robot is oscillating for " << (ros::Time::now() - last_oscillation_reset_).toSec() << "s!";
223  ROS_WARN_STREAM_NAMED("move_base", oscillation_msgs.str());
225 
226  if (attemptRecovery())
227  {
229  }
230  else
231  {
232  mbf_msgs::MoveBaseResult move_base_result;
233  move_base_result.outcome = mbf_msgs::MoveBaseResult::OSCILLATION;
234  move_base_result.message = oscillation_msgs.str();
235  move_base_result.final_pose = robot_pose_;
236  move_base_result.angle_to_goal = move_base_feedback.angle_to_goal;
237  move_base_result.dist_to_goal = move_base_feedback.dist_to_goal;
238  goal_handle_.setAborted(move_base_result, move_base_result.message);
239  }
240  }
241  }
242 }
243 
246  const mbf_msgs::GetPathResultConstPtr &result_ptr)
247 {
248  const mbf_msgs::GetPathResult &get_path_result = *result_ptr;
249  mbf_msgs::MoveBaseResult move_base_result;
250 
251  // copy result from get_path action
252  fillMoveBaseResult(get_path_result, move_base_result);
253 
254  switch (state.state_)
255  {
257  ROS_FATAL_STREAM_NAMED("move_base", "get_path PENDING state not implemented, this should not be reachable!");
258  break;
259 
261  ROS_DEBUG_STREAM_NAMED("move_base", "Action \""
262  << "move_base\" received a path from \""
263  << "get_path\": " << state.getText());
264 
265  exe_path_goal_.path = get_path_result.path;
266  ROS_DEBUG_STREAM_NAMED("move_base", "Action \""
267  << "move_base\" sends the path to \""
268  << "exe_path\".");
269 
271  {
272  ROS_WARN_NAMED("move_base", "Recovered from planner failure: restart recovery behaviors");
275  }
276 
279  boost::bind(&MoveBaseAction::actionExePathDone, this, _1, _2),
280  boost::bind(&MoveBaseAction::actionExePathActive, this),
281  boost::bind(&MoveBaseAction::actionExePathFeedback, this, _1));
283  break;
284 
286 
287  if (attemptRecovery())
288  {
290  }
291  else
292  {
293  // copy result from get_path action
294  ROS_WARN_STREAM_NAMED("move_base", "Abort the execution of the planner: " << get_path_result.message);
295  goal_handle_.setAborted(move_base_result, state.getText());
296  }
298  break;
299 
302  ROS_INFO_STREAM_NAMED("move_base", "The last action goal to \"get_path\" has been " << state.toString());
303  if (action_state_ == CANCELED)
304  {
305  // move_base preempted while executing get_path; fill result and report canceled to the client
306  ROS_INFO_STREAM_NAMED("move_base", "move_base preempted while executing get_path");
307  goal_handle_.setCanceled(move_base_result, state.getText());
308  }
309  break;
310 
312  ROS_ERROR_STREAM_NAMED("move_base", "The last action goal to \"get_path\" has been " << state.toString());
313  goal_handle_.setCanceled(move_base_result, state.getText());
315  break;
316 
318  ROS_FATAL_STREAM_NAMED("move_base", "Connection lost to the action \"get_path\"!");
321  break;
322 
323  default:
324  ROS_FATAL_STREAM_NAMED("move_base", "Reached unknown action server state!");
327  break;
328  }
329 
330  // start replanning if enabled (can be disabled by dynamic reconfigure) and if we started following a path
332  return;
333 
334  // we reset the replan clock (we can have been stopped for a while) and make a fist sleep, so we don't replan
335  // just after start moving
336  boost::lock_guard<boost::mutex> guard(replanning_mtx_);
339  if (!replanning_ || action_state_ != EXE_PATH ||
342  return; // another chance to stop replannings after waiting for replanning period
343  ROS_INFO_STREAM_NAMED("move_base", "Start replanning, using the \"get_path\" action!");
346  boost::bind(&MoveBaseAction::actionGetPathReplanningDone, this, _1, _2)
347  );
348 }
349 
352  const mbf_msgs::ExePathResultConstPtr &result_ptr)
353 {
354  ROS_DEBUG_STREAM_NAMED("move_base", "Action \"exe_path\" finished.");
355 
356  const mbf_msgs::ExePathResult& exe_path_result = *result_ptr;
357  mbf_msgs::MoveBaseResult move_base_result;
358 
359  // copy result from exe_path action
360  fillMoveBaseResult(exe_path_result, move_base_result);
361 
362  ROS_DEBUG_STREAM_NAMED("move_base", "Current state:" << state.toString());
363 
364  switch (state.state_)
365  {
367  move_base_result.outcome = mbf_msgs::MoveBaseResult::SUCCESS;
368  move_base_result.message = "Action \"move_base\" succeeded!";
369  ROS_INFO_STREAM_NAMED("move_base", move_base_result.message);
370  goal_handle_.setSucceeded(move_base_result, move_base_result.message);
372  break;
373 
376 
377  switch (exe_path_result.outcome)
378  {
379  case mbf_msgs::ExePathResult::INVALID_PATH:
380  case mbf_msgs::ExePathResult::TF_ERROR:
381  case mbf_msgs::ExePathResult::NOT_INITIALIZED:
382  case mbf_msgs::ExePathResult::INVALID_PLUGIN:
383  case mbf_msgs::ExePathResult::INTERNAL_ERROR:
384  // none of these errors is recoverable
385  goal_handle_.setAborted(move_base_result, state.getText());
386  break;
387 
388  default:
389  // all the rest are, so we start calling the recovery behaviors in sequence
390 
391  if (attemptRecovery())
392  {
394  }
395  else
396  {
397  ROS_WARN_STREAM_NAMED("move_base", "Abort the execution of the controller: " << exe_path_result.message);
398  goal_handle_.setAborted(move_base_result, state.getText());
399  }
400  break;
401  }
402  break;
403 
406  ROS_INFO_STREAM_NAMED("move_base", "The last action goal to \"exe_path\" has been " << state.toString());
407  if (action_state_ == CANCELED)
408  {
409  // move_base preempted while executing exe_path; fill result and report canceled to the client
410  ROS_INFO_STREAM_NAMED("move_base", "move_base preempted while executing exe_path");
411  goal_handle_.setCanceled(move_base_result, state.getText());
412  }
413  break;
414 
416  ROS_ERROR_STREAM_NAMED("move_base", "The last action goal to \"exe_path\" has been " << state.toString());
417  goal_handle_.setCanceled(move_base_result, state.getText());
419  break;
420 
422  ROS_FATAL_STREAM_NAMED("move_base", "Connection lost to the action \"exe_path\"!");
425  break;
426 
427  default:
428  ROS_FATAL_STREAM_NAMED("move_base", "Reached unreachable case! Unknown SimpleActionServer state!");
431  break;
432  }
433 }
434 
436 {
437  if (!recovery_enabled_)
438  {
439  ROS_WARN_STREAM_NAMED("move_base", "Recovery behaviors are disabled!");
440  return false;
441  }
442 
444  {
445  if (recovery_behaviors_.empty())
446  {
447  ROS_WARN_STREAM_NAMED("move_base", "No Recovery Behaviors loaded!");
448  }
449  else
450  {
451  ROS_WARN_STREAM_NAMED("move_base", "Executed all available recovery behaviors!");
452  }
453  return false;
454  }
455 
457  ROS_DEBUG_STREAM_NAMED("move_base", "Start recovery behavior\""
458  << *current_recovery_behavior_ <<"\".");
461  boost::bind(&MoveBaseAction::actionRecoveryDone, this, _1, _2)
462  );
464  return true;
465 }
466 
469  const mbf_msgs::RecoveryResultConstPtr &result_ptr)
470 {
471  // give the robot some time to stop oscillating after executing the recovery behavior
473 
474  const mbf_msgs::RecoveryResult& recovery_result = *result_ptr;
475  mbf_msgs::MoveBaseResult move_base_result;
476 
477  // copy result from recovery action
478  fillMoveBaseResult(recovery_result, move_base_result);
479 
480  switch (state.state_)
481  {
485 
486  ROS_DEBUG_STREAM_NAMED("move_base", "The recovery behavior \""
487  << *current_recovery_behavior_ << "\" has failed. ");
488  ROS_DEBUG_STREAM("Recovery behavior message: " << recovery_result.message
489  << ", outcome: " << recovery_result.outcome);
490 
491  current_recovery_behavior_++; // use next behavior;
493  {
494  ROS_DEBUG_STREAM_NAMED("move_base",
495  "All recovery behaviors failed. Abort recovering and abort the move_base action");
496  goal_handle_.setAborted(move_base_result, "All recovery behaviors failed.");
497  }
498  else
499  {
501 
502  ROS_INFO_STREAM_NAMED("move_base", "Run the next recovery behavior \""
503  << *current_recovery_behavior_ << "\".");
506  boost::bind(&MoveBaseAction::actionRecoveryDone, this, _1, _2)
507  );
508  }
509  break;
511  //go to planning state
512  ROS_DEBUG_STREAM_NAMED("move_base", "Execution of the recovery behavior \""
513  << *current_recovery_behavior_ << "\" succeeded!");
514  ROS_DEBUG_STREAM_NAMED("move_base",
515  "Try planning again and increment the current recovery behavior in the list.");
517  current_recovery_behavior_++; // use next behavior, the next time;
520  boost::bind(&MoveBaseAction::actionGetPathDone, this, _1, _2)
521  );
522  break;
525  ROS_INFO_STREAM_NAMED("move_base", "The last action goal to \"recovery\" has been preempted!");
526  if (action_state_ == CANCELED)
527  {
528  // move_base preempted while executing a recovery; fill result and report canceled to the client
529  ROS_INFO_STREAM_NAMED("move_base", "move_base preempted while executing a recovery behavior");
530  goal_handle_.setCanceled(move_base_result, state.getText());
531  }
532  break;
533 
535  ROS_FATAL_STREAM_NAMED("move_base", "Connection lost to the action \"recovery\"!");
538  break;
539  default:
540  ROS_FATAL_STREAM_NAMED("move_base", "Reached unreachable case! Unknown state!");
543  break;
544  }
545 }
546 
549  const mbf_msgs::GetPathResultConstPtr &result)
550 {
552  return; // replan only while following a path and if replanning is enabled (can be disabled by dynamic reconfigure)
553 
555  {
556  ROS_DEBUG_STREAM_NAMED("move_base", "Replanning succeeded; sending a goal to \"exe_path\" with the new plan");
557  exe_path_goal_.path = result->path;
558  mbf_msgs::ExePathGoal goal(exe_path_goal_);
560  goal,
561  boost::bind(&MoveBaseAction::actionExePathDone, this, _1, _2),
562  boost::bind(&MoveBaseAction::actionExePathActive, this),
563  boost::bind(&MoveBaseAction::actionExePathFeedback, this, _1));
564  }
565 
566  replanning_mtx_.lock();
568  replanning_mtx_.unlock();
569 
571  return; // another chance to stop replannings after waiting for replanning period
572 
573  ROS_DEBUG_STREAM_NAMED("move_base", "Next replanning cycle, using the \"get_path\" action!");
576  boost::bind(&MoveBaseAction::actionGetPathReplanningDone, this, _1, _2)); // replanning
577 }
578 
579 } /* namespace mbf_abstract_nav */
void publishFeedback(const Feedback &feedback)
const mbf_utility::RobotInformation & robot_info_
current robot state
std::vector< std::string > recovery_behaviors_
#define ROS_INFO_NAMED(name,...)
mbf_msgs::GetPathGoal get_path_goal_
ros::Duration oscillation_timeout_
timeout after a oscillation is detected
#define ROS_DEBUG_STREAM_NAMED(name, args)
geometry_msgs::PoseStamped last_oscillation_pose_
#define ROS_ERROR_STREAM_NAMED(name, args)
boost::shared_ptr< const Goal > getGoal() const
#define ROS_WARN_NAMED(name,...)
void actionExePathDone(const actionlib::SimpleClientGoalState &state, const mbf_msgs::ExePathResultConstPtr &result)
std::string toString() const
#define ROS_INFO_STREAM_NAMED(name, args)
MoveBaseAction(const std::string &name, const mbf_utility::RobotInformation &robot_info, const std::vector< std::string > &controllers)
void setCanceled(const Result &result=Result(), const std::string &text=std::string(""))
ActionClientGetPath action_client_get_path_
Action client used by the move_base action.
void setAccepted(const std::string &text=std::string(""))
bool waitForServer(const ros::Duration &timeout=ros::Duration(0, 0)) const
void reset()
void reconfigure(mbf_abstract_nav::MoveBaseFlexConfig &config, uint32_t level)
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
SimpleClientGoalState getState() const
const std::vector< std::string > & behaviors_
#define ROS_FATAL_STREAM_NAMED(name, args)
mbf_msgs::RecoveryGoal recovery_goal_
void actionRecoveryDone(const actionlib::SimpleClientGoalState &state, const mbf_msgs::RecoveryResultConstPtr &result)
void actionGetPathDone(const actionlib::SimpleClientGoalState &state, const mbf_msgs::GetPathResultConstPtr &result)
#define ROS_DEBUG_STREAM(args)
void fillMoveBaseResult(const ResultType &result, mbf_msgs::MoveBaseResult &move_base_result)
bool sleep()
mbf_msgs::ExePathGoal exe_path_goal_
double distance(const geometry_msgs::PoseStamped &pose1, const geometry_msgs::PoseStamped &pose2)
MoveBaseActionState recovery_trigger_
bool recovery_enabled_
true, if recovery behavior for the MoveBase action is enabled.
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
void sendGoal(const Goal &goal, SimpleDoneCallback done_cb=SimpleDoneCallback(), SimpleActiveCallback active_cb=SimpleActiveCallback(), SimpleFeedbackCallback feedback_cb=SimpleFeedbackCallback())
geometry_msgs::PoseStamped goal_pose_
current goal pose; used to compute remaining distance and angle
bool getRobotPose(geometry_msgs::PoseStamped &robot_pose) const
static Time now()
std::vector< std::string >::iterator current_recovery_behavior_
geometry_msgs::PoseStamped robot_pose_
current robot pose; updated with exe_path action feedback
ActionClientRecovery action_client_recovery_
Action client used by the move_base action.
void actionGetPathReplanningDone(const actionlib::SimpleClientGoalState &state, const mbf_msgs::GetPathResultConstPtr &result)
void actionExePathFeedback(const mbf_msgs::ExePathFeedbackConstPtr &feedback)
ActionClientExePath action_client_exe_path_
Action client used by the move_base action.
void start(GoalHandle &goal_handle)
double oscillation_distance_
minimal move distance to not detect an oscillation
#define ROS_WARN_STREAM_NAMED(name, args)


mbf_abstract_nav
Author(s): Sebastian Pütz
autogenerated on Fri Nov 6 2020 04:05:45