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 
41 #include <limits>
42 
44 
45 #include "mbf_abstract_nav/MoveBaseFlexConfig.h"
47 
48 namespace mbf_abstract_nav
49 {
50 MoveBaseAction::MoveBaseAction(const std::string& name, const mbf_utility::RobotInformation& robot_info,
51  const std::vector<std::string>& behaviors)
52  : name_(name)
53  , robot_info_(robot_info)
54  , private_nh_("~")
55  , action_client_exe_path_(private_nh_, "exe_path")
56  , action_client_get_path_(private_nh_, "get_path")
57  , action_client_recovery_(private_nh_, "recovery")
58  , oscillation_timeout_(0)
59  , oscillation_distance_(0)
60  , recovery_enabled_(true)
61  , behaviors_(behaviors)
62  , action_state_(NONE)
63  , recovery_trigger_(NONE)
64  , dist_to_goal_(std::numeric_limits<double>::infinity())
65  , replanning_thread_(boost::bind(&MoveBaseAction::replanningThread, this))
66 {
67 }
68 
70 {
72  replanning_thread_.join();
73 }
74 
76  mbf_abstract_nav::MoveBaseFlexConfig &config, uint32_t level)
77 {
78  if (config.planner_frequency > 0.0)
79  replanning_period_.fromSec(1.0 / config.planner_frequency);
80  else
82  oscillation_timeout_ = ros::Duration(config.oscillation_timeout);
83  oscillation_distance_ = config.oscillation_distance;
84  recovery_enabled_ = config.recovery_enabled;
85 }
86 
88 {
90 
92  {
94  }
95 
97  {
99  }
100 
102  {
104  }
105 }
106 
108 {
109  dist_to_goal_ = std::numeric_limits<double>::infinity();
110 
112 
113  goal_handle.setAccepted();
114 
115  goal_handle_ = goal_handle;
116 
117  ROS_DEBUG_STREAM_NAMED("move_base", "Start action \"move_base\"");
118 
119  const mbf_msgs::MoveBaseGoal& goal = *goal_handle.getGoal();
120 
121  mbf_msgs::MoveBaseResult move_base_result;
122 
123  get_path_goal_.target_pose = goal.target_pose;
124  get_path_goal_.use_start_pose = false; // use the robot pose
125  get_path_goal_.planner = goal.planner;
126  exe_path_goal_.controller = goal.controller;
127 
128  ros::Duration connection_timeout(1.0);
129 
131 
132  // start recovering with the first behavior, use the recovery behaviors from the action request, if specified,
133  // otherwise, use all loaded behaviors.
134 
135  recovery_behaviors_ = goal.recovery_behaviors.empty() ? behaviors_ : goal.recovery_behaviors;
137 
138  // get the current robot pose only at the beginning, as exe_path will keep updating it as we move
140  {
141  ROS_ERROR_STREAM_NAMED("move_base", "Could not get the current robot pose!");
142  move_base_result.message = "Could not get the current robot pose!";
143  move_base_result.outcome = mbf_msgs::MoveBaseResult::TF_ERROR;
144  goal_handle.setAborted(move_base_result, move_base_result.message);
145  return;
146  }
147  goal_pose_ = goal.target_pose;
148 
149  // wait for server connections
150  if (!action_client_get_path_.waitForServer(connection_timeout) ||
151  !action_client_exe_path_.waitForServer(connection_timeout) ||
152  !action_client_recovery_.waitForServer(connection_timeout))
153  {
154  ROS_ERROR_STREAM_NAMED("move_base", "Could not connect to one or more of move_base_flex actions: "
155  "\"get_path\", \"exe_path\", \"recovery \"!");
156  move_base_result.outcome = mbf_msgs::MoveBaseResult::INTERNAL_ERROR;
157  move_base_result.message = "Could not connect to the move_base_flex actions!";
158  goal_handle.setAborted(move_base_result, move_base_result.message);
159  return;
160  }
161 
162  // call get_path action server to get a first plan
165  boost::bind(&MoveBaseAction::actionGetPathDone, this, _1, _2));
166 }
167 
169 {
170  ROS_DEBUG_STREAM_NAMED("move_base", "The \"exe_path\" action is active.");
171 }
172 
174  const mbf_msgs::ExePathFeedbackConstPtr &feedback)
175 {
176  mbf_msgs::MoveBaseFeedback move_base_feedback;
177  move_base_feedback.outcome = feedback->outcome;
178  move_base_feedback.message = feedback->message;
179  move_base_feedback.angle_to_goal = feedback->angle_to_goal;
180  move_base_feedback.dist_to_goal = feedback->dist_to_goal;
181  move_base_feedback.current_pose = feedback->current_pose;
182  move_base_feedback.last_cmd_vel = feedback->last_cmd_vel;
183  goal_handle_.publishFeedback(move_base_feedback);
184  dist_to_goal_ = feedback->dist_to_goal;
185  robot_pose_ = feedback->current_pose;
186 
187  // we create a navigation-level oscillation detection using exe_path action's feedback,
188  // as the later doesn't handle oscillations created by quickly failing repeated plans
189 
190  // if oscillation detection is enabled by oscillation_timeout != 0
192  {
193  // check if oscillating
194  // moved more than the minimum oscillation distance
196  {
199 
201  {
202  ROS_INFO_NAMED("move_base", "Recovered from robot oscillation: restart recovery behaviors");
205  }
206  }
208  {
209  std::stringstream oscillation_msgs;
210  oscillation_msgs << "Robot is oscillating for " << (ros::Time::now() - last_oscillation_reset_).toSec() << "s!";
211  ROS_WARN_STREAM_NAMED("move_base", oscillation_msgs.str());
213 
214  if (attemptRecovery())
215  {
217  }
218  else
219  {
220  mbf_msgs::MoveBaseResult move_base_result;
221  move_base_result.outcome = mbf_msgs::MoveBaseResult::OSCILLATION;
222  move_base_result.message = oscillation_msgs.str();
223  move_base_result.final_pose = robot_pose_;
224  move_base_result.angle_to_goal = move_base_feedback.angle_to_goal;
225  move_base_result.dist_to_goal = move_base_feedback.dist_to_goal;
226  goal_handle_.setAborted(move_base_result, move_base_result.message);
227  }
228  }
229  }
230 }
231 
234  const mbf_msgs::GetPathResultConstPtr &result_ptr)
235 {
236  const mbf_msgs::GetPathResult &get_path_result = *result_ptr;
237  mbf_msgs::MoveBaseResult move_base_result;
238 
239  // copy result from get_path action
240  fillMoveBaseResult(get_path_result, move_base_result);
241 
242  switch (state.state_)
243  {
245  ROS_FATAL_STREAM_NAMED("move_base", "get_path PENDING state not implemented, this should not be reachable!");
246  break;
247 
249  ROS_DEBUG_STREAM_NAMED("move_base", "Action \""
250  << "move_base\" received a path from \""
251  << "get_path\": " << state.getText());
252 
253  exe_path_goal_.path = get_path_result.path;
254  ROS_DEBUG_STREAM_NAMED("move_base", "Action \""
255  << "move_base\" sends the path to \""
256  << "exe_path\".");
257 
259  {
260  ROS_WARN_NAMED("move_base", "Recovered from planner failure: restart recovery behaviors");
263  }
264 
267  boost::bind(&MoveBaseAction::actionExePathDone, this, _1, _2),
268  boost::bind(&MoveBaseAction::actionExePathActive, this),
269  boost::bind(&MoveBaseAction::actionExePathFeedback, this, _1));
271  break;
272 
274 
275  if (attemptRecovery())
276  {
278  }
279  else
280  {
281  // copy result from get_path action
282  ROS_WARN_STREAM_NAMED("move_base", "Abort the execution of the planner: " << get_path_result.message);
283  goal_handle_.setAborted(move_base_result, state.getText());
284  }
286  break;
287 
290  ROS_INFO_STREAM_NAMED("move_base", "The last action goal to \"get_path\" has been " << state.toString());
291  if (action_state_ == CANCELED)
292  {
293  // move_base preempted while executing get_path; fill result and report canceled to the client
294  ROS_INFO_STREAM_NAMED("move_base", "move_base preempted while executing get_path");
295  goal_handle_.setCanceled(move_base_result, state.getText());
296  }
297  break;
298 
300  ROS_ERROR_STREAM_NAMED("move_base", "The last action goal to \"get_path\" has been " << state.toString());
301  goal_handle_.setCanceled(move_base_result, state.getText());
303  break;
304 
306  ROS_FATAL_STREAM_NAMED("move_base", "Connection lost to the action \"get_path\"!");
309  break;
310 
311  default:
312  ROS_FATAL_STREAM_NAMED("move_base", "Reached unknown action server state!");
315  break;
316  }
317 }
318 
321  const mbf_msgs::ExePathResultConstPtr &result_ptr)
322 {
323  ROS_DEBUG_STREAM_NAMED("move_base", "Action \"exe_path\" finished.");
324 
325  const mbf_msgs::ExePathResult& exe_path_result = *result_ptr;
326  mbf_msgs::MoveBaseResult move_base_result;
327 
328  // copy result from exe_path action
329  fillMoveBaseResult(exe_path_result, move_base_result);
330 
331  ROS_DEBUG_STREAM_NAMED("move_base", "Current state: " << state.toString());
332 
333  switch (state.state_)
334  {
336  move_base_result.outcome = mbf_msgs::MoveBaseResult::SUCCESS;
337  move_base_result.message = "Action \"move_base\" succeeded!";
338  ROS_INFO_STREAM_NAMED("move_base", move_base_result.message);
339  goal_handle_.setSucceeded(move_base_result, move_base_result.message);
341  break;
342 
345 
346  switch (exe_path_result.outcome)
347  {
348  case mbf_msgs::ExePathResult::INVALID_PATH:
349  case mbf_msgs::ExePathResult::TF_ERROR:
350  case mbf_msgs::ExePathResult::NOT_INITIALIZED:
351  case mbf_msgs::ExePathResult::INVALID_PLUGIN:
352  case mbf_msgs::ExePathResult::INTERNAL_ERROR:
353  // none of these errors is recoverable
354  goal_handle_.setAborted(move_base_result, state.getText());
355  break;
356 
357  default:
358  // all the rest are, so we start calling the recovery behaviors in sequence
359 
360  if (attemptRecovery())
361  {
363  }
364  else
365  {
366  ROS_WARN_STREAM_NAMED("move_base", "Abort the execution of the controller: " << exe_path_result.message);
367  goal_handle_.setAborted(move_base_result, state.getText());
368  }
369  break;
370  }
371  break;
372 
375  ROS_INFO_STREAM_NAMED("move_base", "The last action goal to \"exe_path\" has been " << state.toString());
376  if (action_state_ == CANCELED)
377  {
378  // move_base preempted while executing exe_path; fill result and report canceled to the client
379  ROS_INFO_STREAM_NAMED("move_base", "move_base preempted while executing exe_path");
380  goal_handle_.setCanceled(move_base_result, state.getText());
381  }
382  break;
383 
385  ROS_ERROR_STREAM_NAMED("move_base", "The last action goal to \"exe_path\" has been " << state.toString());
386  goal_handle_.setCanceled(move_base_result, state.getText());
388  break;
389 
391  ROS_FATAL_STREAM_NAMED("move_base", "Connection lost to the action \"exe_path\"!");
394  break;
395 
396  default:
397  ROS_FATAL_STREAM_NAMED("move_base", "Reached unreachable case! Unknown SimpleActionServer state!");
400  break;
401  }
402 }
403 
405 {
406  if (!recovery_enabled_)
407  {
408  ROS_WARN_STREAM_NAMED("move_base", "Recovery behaviors are disabled!");
409  return false;
410  }
411 
413  {
414  if (recovery_behaviors_.empty())
415  {
416  ROS_WARN_STREAM_NAMED("move_base", "No Recovery Behaviors loaded!");
417  }
418  else
419  {
420  ROS_WARN_STREAM_NAMED("move_base", "Executed all available recovery behaviors!");
421  }
422  return false;
423  }
424 
426  ROS_DEBUG_STREAM_NAMED("move_base", "Start recovery behavior\""
427  << *current_recovery_behavior_ <<"\".");
430  boost::bind(&MoveBaseAction::actionRecoveryDone, this, _1, _2)
431  );
433  return true;
434 }
435 
438  const mbf_msgs::RecoveryResultConstPtr &result_ptr)
439 {
440  // give the robot some time to stop oscillating after executing the recovery behavior
442 
443  const mbf_msgs::RecoveryResult& recovery_result = *result_ptr;
444  mbf_msgs::MoveBaseResult move_base_result;
445 
446  // copy result from recovery action
447  fillMoveBaseResult(recovery_result, move_base_result);
448 
449  switch (state.state_)
450  {
454 
455  ROS_DEBUG_STREAM_NAMED("move_base", "The recovery behavior \""
456  << *current_recovery_behavior_ << "\" has failed. ");
457  ROS_DEBUG_STREAM("Recovery behavior message: " << recovery_result.message
458  << ", outcome: " << recovery_result.outcome);
459 
460  current_recovery_behavior_++; // use next behavior;
462  {
463  ROS_DEBUG_STREAM_NAMED("move_base",
464  "All recovery behaviors failed. Abort recovering and abort the move_base action");
465  goal_handle_.setAborted(move_base_result, "All recovery behaviors failed.");
466  }
467  else
468  {
470 
471  ROS_INFO_STREAM_NAMED("move_base", "Run the next recovery behavior \""
472  << *current_recovery_behavior_ << "\".");
475  boost::bind(&MoveBaseAction::actionRecoveryDone, this, _1, _2)
476  );
477  }
478  break;
480  //go to planning state
481  ROS_DEBUG_STREAM_NAMED("move_base", "Execution of the recovery behavior \""
482  << *current_recovery_behavior_ << "\" succeeded!");
483  ROS_DEBUG_STREAM_NAMED("move_base",
484  "Try planning again and increment the current recovery behavior in the list.");
486  current_recovery_behavior_++; // use next behavior, the next time;
489  boost::bind(&MoveBaseAction::actionGetPathDone, this, _1, _2)
490  );
491  break;
494  ROS_INFO_STREAM_NAMED("move_base", "The last action goal to \"recovery\" has been preempted!");
495  if (action_state_ == CANCELED)
496  {
497  // move_base preempted while executing a recovery; fill result and report canceled to the client
498  ROS_INFO_STREAM_NAMED("move_base", "move_base preempted while executing a recovery behavior");
499  goal_handle_.setCanceled(move_base_result, state.getText());
500  }
501  break;
502 
504  ROS_FATAL_STREAM_NAMED("move_base", "Connection lost to the action \"recovery\"!");
507  break;
508  default:
509  ROS_FATAL_STREAM_NAMED("move_base", "Reached unreachable case! Unknown state!");
512  break;
513  }
514 }
515 
517 {
518  // replan only while following a path and if replanning is enabled (can be disabled by dynamic reconfigure)
520 }
521 
523 {
524  ros::Duration update_period(0.005);
525  ros::Time last_replan_time(0.0);
526 
527  while (ros::ok())
528  {
530  {
531  if (action_client_get_path_.waitForResult(update_period))
532  {
534  mbf_msgs::GetPathResultConstPtr result = action_client_get_path_.getResult();
536  {
537  ROS_DEBUG_STREAM_NAMED("move_base", "Replanning succeeded; sending a goal to \"exe_path\" with the new plan");
538  exe_path_goal_.path = result->path;
539  mbf_msgs::ExePathGoal goal(exe_path_goal_);
540  action_client_exe_path_.sendGoal(goal, boost::bind(&MoveBaseAction::actionExePathDone, this, _1, _2),
541  boost::bind(&MoveBaseAction::actionExePathActive, this),
542  boost::bind(&MoveBaseAction::actionExePathFeedback, this, _1));
543  }
544  else
545  {
546  ROS_DEBUG_STREAM_NAMED("move_base",
547  "Replanning failed with error code " << result->outcome << ": " << result->message);
548  }
549  }
550  // else keep waiting for planning to complete (we already waited update_period in waitForResult)
551  }
552  else if (!replanningActive())
553  {
554  update_period.sleep();
555  }
556  else if (ros::Time::now() - last_replan_time >= replanning_period_)
557  {
558  ROS_DEBUG_STREAM_NAMED("move_base", "Next replanning cycle, using the \"get_path\" action!");
560  last_replan_time = ros::Time::now();
561  }
562  }
563 }
564 
565 } /* 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_
double dist_to_goal_
current distance to goal (we will stop replanning if very close to avoid destabilizing the controller...
ResultConstPtr getResult() const
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)
boost::thread replanning_thread_
Replanning thread, running permanently.
bool waitForResult(const ros::Duration &timeout=ros::Duration(0, 0))
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 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_
Duration & fromSec(double t)
#define ROS_FATAL_STREAM_NAMED(name, args)
mbf_msgs::RecoveryGoal recovery_goal_
ROSCPP_DECL bool ok()
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)
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
bool sleep() const
ActionClientRecovery action_client_recovery_
Action client used by the move_base action.
void actionExePathFeedback(const mbf_msgs::ExePathFeedbackConstPtr &feedback)
ros::Duration replanning_period_
Replanning period dynamically reconfigurable.
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 Mon Feb 28 2022 22:49:50