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 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().get());
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  geometry_msgs::PoseStamped robot_pose;
152  // get the current robot pose only at the beginning, as exe_path will keep updating it as we move
153  if (!robot_info_.getRobotPose(robot_pose))
154  {
155  ROS_ERROR_STREAM_NAMED("move_base", "Could not get the current robot pose!");
156  move_base_result.message = "Could not get the current robot pose!";
157  move_base_result.outcome = mbf_msgs::MoveBaseResult::TF_ERROR;
158  goal_handle.setAborted(move_base_result, move_base_result.message);
159  return;
160  }
161  goal_pose_ = goal.target_pose;
162 
163  // wait for server connections
164  if (!action_client_get_path_.waitForServer(connection_timeout) ||
165  !action_client_exe_path_.waitForServer(connection_timeout) ||
166  !action_client_recovery_.waitForServer(connection_timeout))
167  {
168  ROS_ERROR_STREAM_NAMED("move_base", "Could not connect to one or more of move_base_flex actions:"
169  "\"get_path\" , \"exe_path\", \"recovery \"!");
170  move_base_result.outcome = mbf_msgs::MoveBaseResult::INTERNAL_ERROR;
171  move_base_result.message = "Could not connect to the move_base_flex actions!";
172  goal_handle.setAborted(move_base_result, move_base_result.message);
173  return;
174  }
175 
176  // call get_path action server to get a first plan
179  boost::bind(&MoveBaseAction::actionGetPathDone, this, _1, _2));
180 }
181 
183 {
184  ROS_DEBUG_STREAM_NAMED("move_base", "The \"exe_path\" action is active.");
185 }
186 
188  const mbf_msgs::ExePathFeedbackConstPtr &feedback)
189 {
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;
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 osciallation_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("exe_path", 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  if (action_state_ == CANCELED)
249  return;
250 
252 
253  const mbf_msgs::GetPathResult &result = *(result_ptr.get());
254  mbf_msgs::MoveBaseResult move_base_result;
255  switch (state.state_)
256  {
258  ROS_FATAL_STREAM_NAMED("move_base", "get_path PENDING state not implemented, this should not be reachable!");
259  break;
260 
262  ROS_DEBUG_STREAM_NAMED("move_base", "Action \""
263  << "move_base\" received a path from \""
264  << "get_path\": " << state.getText());
265 
266  exe_path_goal_.path = result.path;
267  ROS_DEBUG_STREAM_NAMED("move_base", "Action \""
268  << "move_base\" sends the path to \""
269  << "exe_path\".");
270 
272  {
273  ROS_WARN_NAMED("move_base", "Recovered from planner failure: restart recovery behaviors");
276  }
277 
280  boost::bind(&MoveBaseAction::actionExePathDone, this, _1, _2),
281  boost::bind(&MoveBaseAction::actionExePathActive, this),
282  boost::bind(&MoveBaseAction::actionExePathFeedback, this, _1));
284  break;
285 
287 
288  if (attemptRecovery())
289  {
291  }
292  else
293  {
294  // copy result from get_path action
295  move_base_result.outcome = result.outcome;
296  move_base_result.message = result.message;
297  move_base_result.dist_to_goal = static_cast<float>(mbf_utility::distance(robot_pose_, goal_pose_));
298  move_base_result.angle_to_goal = static_cast<float>(mbf_utility::angle(robot_pose_, goal_pose_));
299  move_base_result.final_pose = robot_pose_;
300 
301  ROS_WARN_STREAM_NAMED("move_base", "Abort the execution of the planner: " << result.message);
302  goal_handle_.setAborted(move_base_result, state.getText());
303  }
304  break;
305 
307  // the get_path action has been preempted.
308 
309  // copy result from get_path action
310  move_base_result.outcome = result.outcome;
311  move_base_result.message = result.message;
312  move_base_result.dist_to_goal = static_cast<float>(mbf_utility::distance(robot_pose_, goal_pose_));
313  move_base_result.angle_to_goal = static_cast<float>(mbf_utility::angle(robot_pose_, goal_pose_));
314  move_base_result.final_pose = robot_pose_;
315  goal_handle_.setCanceled(move_base_result, state.getText());
316  break;
317 
320  ROS_FATAL_STREAM_NAMED("move_base", "The states RECALLED and REJECTED are not implemented in the SimpleActionServer!");
322  break;
323 
325  ROS_FATAL_STREAM_NAMED("move_base", "Connection lost to the action \"get_path\"!");
327  break;
328 
329  default:
330  ROS_FATAL_STREAM_NAMED("move_base", "Reached unknown action server state!");
332  break;
333  }
334 
335  // start replanning if enabled (can be disabled by dynamic reconfigure) and if we started following a path
337  return;
338 
339  // we reset the replan clock (we can have been stopped for a while) and make a fist sleep, so we don't replan
340  // just after start moving
341  boost::lock_guard<boost::mutex> guard(replanning_mtx_);
344  if (!replanning_ || action_state_ != EXE_PATH ||
347  return; // another chance to stop replannings after waiting for replanning period
348  ROS_INFO_STREAM_NAMED("move_base", "Start replanning, using the \"get_path\" action!");
351  boost::bind(&MoveBaseAction::actionGetPathReplanningDone, this, _1, _2)
352  );
353 }
354 
357  const mbf_msgs::ExePathResultConstPtr &result_ptr)
358 {
359  if (action_state_ == CANCELED)
360  return;
361 
363 
364  ROS_DEBUG_STREAM_NAMED("move_base", "Action \"exe_path\" finished.");
365 
366  const mbf_msgs::ExePathResult& result = *(result_ptr.get());
367  mbf_msgs::MoveBaseResult move_base_result;
368 
369  // copy result from get_path action
370  move_base_result.outcome = result.outcome;
371  move_base_result.message = result.message;
372  move_base_result.dist_to_goal = result.dist_to_goal;
373  move_base_result.angle_to_goal = result.angle_to_goal;
374  move_base_result.final_pose = result.final_pose;
375 
376  ROS_DEBUG_STREAM_NAMED("exe_path", "Current state:" << state.toString());
377 
378  switch (state.state_)
379  {
381  move_base_result.outcome = mbf_msgs::MoveBaseResult::SUCCESS;
382  move_base_result.message = "Action \"move_base\" succeeded!";
383  ROS_INFO_STREAM_NAMED("move_base", move_base_result.message);
384  goal_handle_.setSucceeded(move_base_result, move_base_result.message);
386  break;
387 
389  switch (result.outcome)
390  {
391  case mbf_msgs::ExePathResult::INVALID_PATH:
392  case mbf_msgs::ExePathResult::TF_ERROR:
393  case mbf_msgs::ExePathResult::NOT_INITIALIZED:
394  case mbf_msgs::ExePathResult::INVALID_PLUGIN:
395  case mbf_msgs::ExePathResult::INTERNAL_ERROR:
396  // none of these errors is recoverable
397  goal_handle_.setAborted(move_base_result, state.getText());
398  break;
399 
400  default:
401  // all the rest are, so we start calling the recovery behaviors in sequence
402 
403  if (attemptRecovery())
404  {
406  }
407  else
408  {
409  ROS_WARN_STREAM_NAMED("move_base", "Abort the execution of the controller: " << result.message);
410  goal_handle_.setAborted(move_base_result, state.getText());
411  }
412  break;
413  }
414  break;
415 
417  // action was preempted successfully!
418  ROS_DEBUG_STREAM_NAMED("move_base", "The action \""
419  << "exe_path" << "\" was preempted successfully!");
420  // TODO
421  break;
422 
424  ROS_DEBUG_STREAM_NAMED("move_base", "The action \""
425  << "exe_path" << "\" was recalled!");
426  // TODO
427  break;
428 
430  ROS_DEBUG_STREAM_NAMED("move_base", "The action \""
431  << "exe_path" << "\" was rejected!");
432  // TODO
433  break;
434 
436  // TODO
437  break;
438 
439  default:
440  ROS_FATAL_STREAM_NAMED("move_base",
441  "Reached unreachable case! Unknown SimpleActionServer state!");
443  break;
444  }
445 }
446 
448 {
449  if (!recovery_enabled_)
450  {
451  ROS_WARN_STREAM_NAMED("move_base", "Recovery behaviors are disabled!");
452  return false;
453  }
454 
456  {
457  if (recovery_behaviors_.empty())
458  {
459  ROS_WARN_STREAM_NAMED("move_base", "No Recovery Behaviors loaded!");
460  }
461  else
462  {
463  ROS_WARN_STREAM_NAMED("move_base", "Executed all available recovery behaviors!");
464  }
465  return false;
466  }
467 
469  ROS_DEBUG_STREAM_NAMED("move_base", "Start recovery behavior\""
470  << *current_recovery_behavior_ <<"\".");
473  boost::bind(&MoveBaseAction::actionRecoveryDone, this, _1, _2)
474  );
476  return true;
477 }
478 
481  const mbf_msgs::RecoveryResultConstPtr &result_ptr)
482 {
483  // give the robot some time to stop oscillating after executing the recovery behavior
485 
486  if (action_state_ == CANCELED)
487  return;
488 
489  action_state_ = FAILED; // unless recovery succeeds or gets canceled...
490 
491  const mbf_msgs::RecoveryResult& result = *(result_ptr.get());
492  mbf_msgs::MoveBaseResult move_base_result;
493 
494  // copy result from get_path action
495  move_base_result.outcome = result.outcome;
496  move_base_result.message = result.message;
497  move_base_result.dist_to_goal = static_cast<float>(mbf_utility::distance(robot_pose_, goal_pose_));
498  move_base_result.angle_to_goal = static_cast<float>(mbf_utility::angle(robot_pose_, goal_pose_));
499  move_base_result.final_pose = robot_pose_;
500 
501  switch (state.state_)
502  {
504  ROS_DEBUG_STREAM_NAMED("move_base", "Recovery behavior aborted!");
505  ROS_DEBUG_STREAM_NAMED("move_base", "The recovery behavior \""
506  << *current_recovery_behavior_ << "\" failed. ");
507  ROS_DEBUG_STREAM("Recovery behavior message: " << result.message
508  << ", outcome: " << result.outcome);
509 
510  current_recovery_behavior_++; // use next behavior;
512  {
513  ROS_DEBUG_STREAM_NAMED("move_base",
514  "All recovery behaviors failed. Abort recovering and abort the move_base action");
515  goal_handle_.setAborted(move_base_result, "All recovery behaviors failed.");
516  }
517  else
518  {
520 
521  ROS_INFO_STREAM_NAMED("move_base", "Run the next recovery behavior\""
522  << *current_recovery_behavior_ << "\".");
525  boost::bind(&MoveBaseAction::actionRecoveryDone, this, _1, _2)
526  );
527  }
528  break;
530  //go to planning state
531  ROS_DEBUG_STREAM_NAMED("move_base", "Execution of the recovery behavior \""
532  << *current_recovery_behavior_ << "\" succeeded!");
533  ROS_DEBUG_STREAM_NAMED("move_base",
534  "Try planning again and increment the current recovery behavior in the list.");
536  current_recovery_behavior_++; // use next behavior, the next time;
539  boost::bind(&MoveBaseAction::actionGetPathDone, this, _1, _2)
540  );
541  break;
543  ROS_INFO_STREAM_NAMED("move_base",
544  "The recovery action has been preempted!");
545  if(action_state_ == CANCELED)
547  break;
548 
550  ROS_INFO_STREAM_NAMED("move_base",
551  "The recovery action has been recalled!");
552  if(action_state_ == CANCELED)
554  break;
555 
557  ROS_FATAL_STREAM_NAMED("move_base",
558  "The recovery action has been rejected!");
560  break;
562  ROS_FATAL_STREAM_NAMED("move_base",
563  "The recovery action has lost the connection to the server!");
565  break;
566  default:
567  ROS_FATAL_STREAM_NAMED("move_base",
568  "Reached unreachable case! Unknown state!");
570  break;
571  }
572 }
573 
576  const mbf_msgs::GetPathResultConstPtr &result)
577 {
579  return; // replan only while following a path and if replanning is enabled (can be disabled by dynamic reconfigure)
580 
582  {
583  ROS_DEBUG_STREAM_NAMED("move_base", "Replanning succeeded; sending a goal to \"exe_path\" with the new plan");
584  exe_path_goal_.path = result->path;
585  mbf_msgs::ExePathGoal goal(exe_path_goal_);
587  goal,
588  boost::bind(&MoveBaseAction::actionExePathDone, this, _1, _2),
589  boost::bind(&MoveBaseAction::actionExePathActive, this),
590  boost::bind(&MoveBaseAction::actionExePathFeedback, this, _1));
591  }
592 
593  replanning_mtx_.lock();
595  replanning_mtx_.unlock();
596 
598  return; // another chance to stop replannings after waiting for replanning period
599 
600  ROS_DEBUG_STREAM_NAMED("move_base", "Next replanning cycle, using the \"get_path\" action!");
603  boost::bind(&MoveBaseAction::actionGetPathReplanningDone, this, _1, _2)); // replanning
604 }
605 
606 } /* namespace mbf_abstract_nav */
void publishFeedback(const Feedback &feedback)
double angle(const geometry_msgs::PoseStamped &pose1, const geometry_msgs::PoseStamped &pose2)
std::vector< std::string > recovery_behaviors_
#define ROS_INFO_NAMED(name,...)
mbf_msgs::GetPathGoal get_path_goal_
mbf_msgs::MoveBaseFeedback move_base_feedback_
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
void setRejected(const Result &result=Result(), const std::string &text=std::string(""))
MoveBaseAction(const std::string &name, const RobotInformation &robot_info, const std::vector< std::string > &controllers)
#define ROS_INFO_STREAM_NAMED(name, args)
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_
bool getRobotPose(geometry_msgs::PoseStamped &robot_pose) const
Computes the current robot pose (robot_frame_) in the global frame (global_frame_).
#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)
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
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 Wed May 27 2020 04:03:14