00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041 #include <mbf_utility/navigation_utility.h>
00042
00043 #include "mbf_abstract_nav/MoveBaseFlexConfig.h"
00044 #include "mbf_abstract_nav/move_base_action.h"
00045
00046 namespace mbf_abstract_nav
00047 {
00048
00049 MoveBaseAction::MoveBaseAction(const std::string &name,
00050 const RobotInformation &robot_info,
00051 const std::vector<std::string> &behaviors)
00052 : name_(name), robot_info_(robot_info), private_nh_("~"),
00053 action_client_exe_path_(private_nh_, "exe_path"),
00054 action_client_get_path_(private_nh_, "get_path"),
00055 action_client_recovery_(private_nh_, "recovery"),
00056 oscillation_timeout_(0),
00057 oscillation_distance_(0),
00058 recovery_enabled_(true),
00059 behaviors_(behaviors),
00060 action_state_(NONE),
00061 recovery_trigger_(NONE),
00062 replanning_(false),
00063 replanning_rate_(1.0)
00064 {
00065 }
00066
00067 MoveBaseAction::~MoveBaseAction()
00068 {
00069 }
00070
00071 void MoveBaseAction::reconfigure(
00072 mbf_abstract_nav::MoveBaseFlexConfig &config, uint32_t level)
00073 {
00074 if (config.planner_frequency > 0.0)
00075 {
00076 boost::lock_guard<boost::mutex> guard(replanning_mtx_);
00077 if (!replanning_)
00078 {
00079 replanning_ = true;
00080 if (action_state_ == EXE_PATH &&
00081 action_client_get_path_.getState() != actionlib::SimpleClientGoalState::PENDING &&
00082 action_client_get_path_.getState() != actionlib::SimpleClientGoalState::ACTIVE)
00083 {
00084
00085 ROS_INFO_STREAM_NAMED("move_base", "Planner frequency set to " << config.planner_frequency
00086 << ": start replanning, using the \"get_path\" action!");
00087 action_client_get_path_.sendGoal(
00088 get_path_goal_,
00089 boost::bind(&MoveBaseAction::actionGetPathReplanningDone, this, _1, _2)
00090 );
00091 }
00092 }
00093 replanning_rate_ = ros::Rate(config.planner_frequency);
00094 }
00095 else
00096 replanning_ = false;
00097 oscillation_timeout_ = ros::Duration(config.oscillation_timeout);
00098 oscillation_distance_ = config.oscillation_distance;
00099 recovery_enabled_ = config.recovery_enabled;
00100 }
00101
00102 void MoveBaseAction::cancel()
00103 {
00104 action_state_ = CANCELED;
00105
00106 if(!action_client_get_path_.getState().isDone())
00107 {
00108 action_client_get_path_.cancelGoal();
00109 }
00110
00111 if(!action_client_exe_path_.getState().isDone())
00112 {
00113 action_client_exe_path_.cancelGoal();
00114 }
00115
00116 if(!action_client_recovery_.getState().isDone())
00117 {
00118 action_client_recovery_.cancelGoal();
00119 }
00120 }
00121
00122 void MoveBaseAction::start(GoalHandle &goal_handle)
00123 {
00124 action_state_ = GET_PATH;
00125
00126 goal_handle.setAccepted();
00127
00128 goal_handle_ = goal_handle;
00129
00130 ROS_DEBUG_STREAM_NAMED("move_base", "Start action " << "move_base");
00131
00132 const mbf_msgs::MoveBaseGoal& goal = *(goal_handle.getGoal().get());
00133
00134 mbf_msgs::MoveBaseResult move_base_result;
00135
00136 get_path_goal_.target_pose = goal.target_pose;
00137 get_path_goal_.use_start_pose = false;
00138 get_path_goal_.planner = goal.planner;
00139 exe_path_goal_.controller = goal.controller;
00140
00141 ros::Duration connection_timeout(1.0);
00142
00143 last_oscillation_reset_ = ros::Time::now();
00144
00145
00146
00147
00148 recovery_behaviors_ = goal.recovery_behaviors.empty() ? behaviors_ : goal.recovery_behaviors;
00149 current_recovery_behavior_ = recovery_behaviors_.begin();
00150
00151 geometry_msgs::PoseStamped robot_pose;
00152
00153 if (!robot_info_.getRobotPose(robot_pose))
00154 {
00155 ROS_ERROR_STREAM_NAMED("move_base", "Could not get the current robot pose!");
00156 move_base_result.message = "Could not get the current robot pose!";
00157 move_base_result.outcome = mbf_msgs::MoveBaseResult::TF_ERROR;
00158 goal_handle.setAborted(move_base_result, move_base_result.message);
00159 return;
00160 }
00161
00162
00163 if (!action_client_get_path_.waitForServer(connection_timeout) ||
00164 !action_client_exe_path_.waitForServer(connection_timeout) ||
00165 !action_client_recovery_.waitForServer(connection_timeout))
00166 {
00167 ROS_ERROR_STREAM_NAMED("move_base", "Could not connect to one or more of move_base_flex actions:"
00168 "\"get_path\" , \"exe_path\", \"recovery \"!");
00169 move_base_result.outcome = mbf_msgs::MoveBaseResult::INTERNAL_ERROR;
00170 move_base_result.message = "Could not connect to the move_base_flex actions!";
00171 goal_handle.setAborted(move_base_result, move_base_result.message);
00172 return;
00173 }
00174
00175
00176 action_client_get_path_.sendGoal(
00177 get_path_goal_,
00178 boost::bind(&MoveBaseAction::actionGetPathDone, this, _1, _2));
00179 }
00180
00181 void MoveBaseAction::actionExePathActive()
00182 {
00183 ROS_DEBUG_STREAM_NAMED("move_base", "The \"exe_path\" action is active.");
00184 }
00185
00186 void MoveBaseAction::actionExePathFeedback(
00187 const mbf_msgs::ExePathFeedbackConstPtr &feedback)
00188 {
00189 move_base_feedback_.outcome = feedback->outcome;
00190 move_base_feedback_.message = feedback->message;
00191 move_base_feedback_.angle_to_goal = feedback->angle_to_goal;
00192 move_base_feedback_.dist_to_goal = feedback->dist_to_goal;
00193 move_base_feedback_.current_pose = feedback->current_pose;
00194 move_base_feedback_.last_cmd_vel = feedback->last_cmd_vel;
00195 robot_pose_ = feedback->current_pose;
00196 goal_handle_.publishFeedback(move_base_feedback_);
00197
00198
00199
00200
00201
00202 if (!oscillation_timeout_.isZero())
00203 {
00204
00205
00206 if (mbf_utility::distance(robot_pose_, last_oscillation_pose_) >= oscillation_distance_)
00207 {
00208 last_oscillation_reset_ = ros::Time::now();
00209 last_oscillation_pose_ = robot_pose_;
00210
00211 if (recovery_trigger_ == OSCILLATING)
00212 {
00213 ROS_INFO_NAMED("move_base", "Recovered from robot oscillation: restart recovery behaviors");
00214 current_recovery_behavior_ = recovery_behaviors_.begin();
00215 recovery_trigger_ = NONE;
00216 }
00217 }
00218 else if (last_oscillation_reset_ + oscillation_timeout_ < ros::Time::now())
00219 {
00220 std::stringstream oscillation_msgs;
00221 oscillation_msgs << "Robot is oscillating for " << ((ros::Time::now() - last_oscillation_reset_).toSec()) << "s!";
00222 ROS_WARN_STREAM_NAMED("exe_path", oscillation_msgs.str());
00223 action_client_exe_path_.cancelGoal();
00224
00225 if (attemptRecovery())
00226 {
00227 recovery_trigger_ = OSCILLATING;
00228 }
00229 else
00230 {
00231 mbf_msgs::MoveBaseResult move_base_result;
00232 move_base_result.outcome = OSCILLATING;
00233 if(recovery_enabled_)
00234 move_base_result.message = oscillation_msgs.str() + " No recovery behaviors for the move_base action are defined!";
00235 else
00236 move_base_result.message = oscillation_msgs.str() + " Recovery is disabled for the move_base action! use the param \"enable_recovery\"";
00237 move_base_result.final_pose = robot_pose_;
00238 move_base_result.angle_to_goal = move_base_feedback_.angle_to_goal;
00239 move_base_result.dist_to_goal = move_base_feedback_.dist_to_goal;
00240 goal_handle_.setAborted(move_base_result, move_base_result.message);
00241 }
00242 }
00243 }
00244 }
00245
00246 void MoveBaseAction::actionGetPathDone(
00247 const actionlib::SimpleClientGoalState &state,
00248 const mbf_msgs::GetPathResultConstPtr &result_ptr)
00249 {
00250 action_state_ = FAILED;
00251
00252 const mbf_msgs::GetPathResult &result = *(result_ptr.get());
00253 const mbf_msgs::MoveBaseGoal& goal = *(goal_handle_.getGoal().get());
00254 mbf_msgs::MoveBaseResult move_base_result;
00255 switch (state.state_)
00256 {
00257 case actionlib::SimpleClientGoalState::PENDING:
00258 ROS_FATAL_STREAM_NAMED("move_base", "get_path PENDING state not implemented, this should not be reachable!");
00259 break;
00260
00261 case actionlib::SimpleClientGoalState::SUCCEEDED:
00262 ROS_DEBUG_STREAM_NAMED("move_base", "Action \""
00263 << "move_base\" received a path from \""
00264 << "get_path\": " << state.getText());
00265
00266 exe_path_goal_.path = result.path;
00267 ROS_DEBUG_STREAM_NAMED("move_base", "Action \""
00268 << "move_base\" sends the path to \""
00269 << "exe_path\".");
00270
00271 if (recovery_trigger_ == GET_PATH)
00272 {
00273 ROS_WARN_NAMED("move_base", "Recovered from planner failure: restart recovery behaviors");
00274 current_recovery_behavior_ = recovery_behaviors_.begin();
00275 recovery_trigger_ = NONE;
00276 }
00277
00278 action_client_exe_path_.sendGoal(
00279 exe_path_goal_,
00280 boost::bind(&MoveBaseAction::actionExePathDone, this, _1, _2),
00281 boost::bind(&MoveBaseAction::actionExePathActive, this),
00282 boost::bind(&MoveBaseAction::actionExePathFeedback, this, _1));
00283 action_state_ = EXE_PATH;
00284 break;
00285
00286 case actionlib::SimpleClientGoalState::ABORTED:
00287
00288 if (attemptRecovery())
00289 {
00290 recovery_trigger_ = GET_PATH;
00291 }
00292 else
00293 {
00294
00295 move_base_result.outcome = result.outcome;
00296 move_base_result.message = result.message;
00297 move_base_result.dist_to_goal = static_cast<float>(mbf_utility::distance(robot_pose_, goal.target_pose));
00298 move_base_result.angle_to_goal = static_cast<float>(mbf_utility::angle(robot_pose_, goal.target_pose));
00299 move_base_result.final_pose = robot_pose_;
00300
00301 ROS_WARN_STREAM_NAMED("move_base", "Abort the execution of the planner: " << result.message);
00302 goal_handle_.setAborted(move_base_result, state.getText());
00303 }
00304 break;
00305
00306 case actionlib::SimpleClientGoalState::PREEMPTED:
00307
00308
00309
00310 move_base_result.outcome = result.outcome;
00311 move_base_result.message = result.message;
00312 move_base_result.dist_to_goal = static_cast<float>(mbf_utility::distance(robot_pose_, goal.target_pose));
00313 move_base_result.angle_to_goal = static_cast<float>(mbf_utility::angle(robot_pose_, goal.target_pose));
00314 move_base_result.final_pose = robot_pose_;
00315 goal_handle_.setCanceled(move_base_result, state.getText());
00316 break;
00317
00318 case actionlib::SimpleClientGoalState::RECALLED:
00319 case actionlib::SimpleClientGoalState::REJECTED:
00320 ROS_FATAL_STREAM_NAMED("move_base", "The states RECALLED and REJECTED are not implemented in the SimpleActionServer!");
00321 goal_handle_.setAborted();
00322 break;
00323
00324 case actionlib::SimpleClientGoalState::LOST:
00325 ROS_FATAL_STREAM_NAMED("move_base", "Connection lost to the action \"get_path\"!");
00326 goal_handle_.setAborted();
00327 break;
00328
00329 default:
00330 ROS_FATAL_STREAM_NAMED("move_base", "Reached unknown action server state!");
00331 goal_handle_.setAborted();
00332 break;
00333 }
00334
00335
00336 if (!replanning_ || action_state_ != EXE_PATH)
00337 return;
00338
00339
00340
00341 boost::lock_guard<boost::mutex> guard(replanning_mtx_);
00342 replanning_rate_.reset();
00343 replanning_rate_.sleep();
00344 if (!replanning_ || action_state_ != EXE_PATH ||
00345 action_client_get_path_.getState() == actionlib::SimpleClientGoalState::PENDING ||
00346 action_client_get_path_.getState() == actionlib::SimpleClientGoalState::ACTIVE)
00347 return;
00348 ROS_INFO_STREAM_NAMED("move_base", "Start replanning, using the \"get_path\" action!");
00349 action_client_get_path_.sendGoal(
00350 get_path_goal_,
00351 boost::bind(&MoveBaseAction::actionGetPathReplanningDone, this, _1, _2)
00352 );
00353 }
00354
00355 void MoveBaseAction::actionExePathDone(
00356 const actionlib::SimpleClientGoalState &state,
00357 const mbf_msgs::ExePathResultConstPtr &result_ptr)
00358 {
00359 action_state_ = FAILED;
00360
00361 ROS_DEBUG_STREAM_NAMED("move_base", "Action \"exe_path\" finished.");
00362
00363 const mbf_msgs::ExePathResult& result = *(result_ptr.get());
00364 const mbf_msgs::MoveBaseGoal& goal = *(goal_handle_.getGoal().get());
00365 mbf_msgs::MoveBaseResult move_base_result;
00366
00367
00368 move_base_result.outcome = result.outcome;
00369 move_base_result.message = result.message;
00370 move_base_result.dist_to_goal = result.dist_to_goal;
00371 move_base_result.angle_to_goal = result.angle_to_goal;
00372 move_base_result.final_pose = result.final_pose;
00373
00374 ROS_DEBUG_STREAM_NAMED("exe_path", "Current state:" << state.toString());
00375
00376 switch (state.state_)
00377 {
00378 case actionlib::SimpleClientGoalState::SUCCEEDED:
00379 move_base_result.outcome = mbf_msgs::MoveBaseResult::SUCCESS;
00380 move_base_result.message = "Action \"move_base\" succeeded!";
00381 ROS_INFO_STREAM_NAMED("move_base", move_base_result.message);
00382 goal_handle_.setSucceeded(move_base_result, move_base_result.message);
00383 action_state_ = SUCCEEDED;
00384 break;
00385
00386 case actionlib::SimpleClientGoalState::ABORTED:
00387 switch (result.outcome)
00388 {
00389 case mbf_msgs::ExePathResult::INVALID_PATH:
00390 case mbf_msgs::ExePathResult::TF_ERROR:
00391 case mbf_msgs::ExePathResult::NOT_INITIALIZED:
00392 case mbf_msgs::ExePathResult::INVALID_PLUGIN:
00393 case mbf_msgs::ExePathResult::INTERNAL_ERROR:
00394
00395 goal_handle_.setAborted(move_base_result, state.getText());
00396 break;
00397
00398 default:
00399
00400
00401 if (attemptRecovery())
00402 {
00403 recovery_trigger_ = EXE_PATH;
00404 }
00405 else
00406 {
00407 ROS_WARN_STREAM_NAMED("move_base", "Abort the execution of the controller: " << result.message);
00408 goal_handle_.setAborted(move_base_result, state.getText());
00409 }
00410 break;
00411 }
00412 break;
00413
00414 case actionlib::SimpleClientGoalState::PREEMPTED:
00415
00416 ROS_DEBUG_STREAM_NAMED("move_base", "The action \""
00417 << "exe_path" << "\" was preempted successfully!");
00418
00419 break;
00420
00421 case actionlib::SimpleClientGoalState::RECALLED:
00422 ROS_DEBUG_STREAM_NAMED("move_base", "The action \""
00423 << "exe_path" << "\" was recalled!");
00424
00425 break;
00426
00427 case actionlib::SimpleClientGoalState::REJECTED:
00428 ROS_DEBUG_STREAM_NAMED("move_base", "The action \""
00429 << "exe_path" << "\" was rejected!");
00430
00431 break;
00432
00433 case actionlib::SimpleClientGoalState::LOST:
00434
00435 break;
00436
00437 default:
00438 ROS_FATAL_STREAM_NAMED("move_base",
00439 "Reached unreachable case! Unknown SimpleActionServer state!");
00440 goal_handle_.setAborted();
00441 break;
00442 }
00443 }
00444
00445 bool MoveBaseAction::attemptRecovery()
00446 {
00447 if (!recovery_enabled_)
00448 {
00449 ROS_WARN_STREAM_NAMED("move_base", "Recovery behaviors are disabled!");
00450 return false;
00451 }
00452
00453 if (current_recovery_behavior_ == recovery_behaviors_.end())
00454 {
00455 if (recovery_behaviors_.empty())
00456 {
00457 ROS_WARN_STREAM_NAMED("move_base", "No Recovery Behaviors loaded!");
00458 }
00459 else
00460 {
00461 ROS_WARN_STREAM_NAMED("move_base", "Executed all available recovery behaviors!");
00462 }
00463 return false;
00464 }
00465
00466 recovery_goal_.behavior = *current_recovery_behavior_;
00467 ROS_DEBUG_STREAM_NAMED("move_base", "Start recovery behavior\""
00468 << *current_recovery_behavior_ <<"\".");
00469 action_client_recovery_.sendGoal(
00470 recovery_goal_,
00471 boost::bind(&MoveBaseAction::actionRecoveryDone, this, _1, _2)
00472 );
00473 action_state_ = RECOVERY;
00474 return true;
00475 }
00476
00477 void MoveBaseAction::actionRecoveryDone(
00478 const actionlib::SimpleClientGoalState &state,
00479 const mbf_msgs::RecoveryResultConstPtr &result_ptr)
00480 {
00481 action_state_ = FAILED;
00482
00483 const mbf_msgs::RecoveryResult& result = *(result_ptr.get());
00484 const mbf_msgs::MoveBaseGoal& goal = *(goal_handle_.getGoal().get());
00485 mbf_msgs::MoveBaseResult move_base_result;
00486
00487
00488 move_base_result.outcome = result.outcome;
00489 move_base_result.message = result.message;
00490 move_base_result.dist_to_goal = static_cast<float>(mbf_utility::distance(robot_pose_, goal.target_pose));
00491 move_base_result.angle_to_goal = static_cast<float>(mbf_utility::angle(robot_pose_, goal.target_pose));
00492 move_base_result.final_pose = robot_pose_;
00493
00494 switch (state.state_)
00495 {
00496 case actionlib::SimpleClientGoalState::ABORTED:
00497 ROS_DEBUG_STREAM_NAMED("move_base", "Recovery behavior aborted!");
00498 ROS_DEBUG_STREAM_NAMED("move_base", "The recovery behavior \""
00499 << *current_recovery_behavior_ << "\" failed. ");
00500 ROS_DEBUG_STREAM("Recovery behavior message: " << result.message
00501 << ", outcome: " << result.outcome);
00502
00503 current_recovery_behavior_++;
00504 if (current_recovery_behavior_ == recovery_behaviors_.end())
00505 {
00506 ROS_DEBUG_STREAM_NAMED("move_base",
00507 "All recovery behaviors failed. Abort recovering and abort the move_base action");
00508 goal_handle_.setAborted(move_base_result, "All recovery behaviors failed.");
00509 }
00510 else
00511 {
00512 recovery_goal_.behavior = *current_recovery_behavior_;
00513
00514 ROS_INFO_STREAM_NAMED("move_base", "Run the next recovery behavior\""
00515 << *current_recovery_behavior_ << "\".");
00516 action_client_recovery_.sendGoal(
00517 recovery_goal_,
00518 boost::bind(&MoveBaseAction::actionRecoveryDone, this, _1, _2)
00519 );
00520 }
00521 break;
00522 case actionlib::SimpleClientGoalState::SUCCEEDED:
00523
00524 ROS_DEBUG_STREAM_NAMED("move_base", "Execution of the recovery behavior \""
00525 << *current_recovery_behavior_ << "\" succeeded!");
00526 ROS_DEBUG_STREAM_NAMED("move_base",
00527 "Try planning again and increment the current recovery behavior in the list.");
00528 action_state_ = GET_PATH;
00529 current_recovery_behavior_++;
00530 action_client_get_path_.sendGoal(
00531 get_path_goal_,
00532 boost::bind(&MoveBaseAction::actionGetPathDone, this, _1, _2)
00533 );
00534 break;
00535 case actionlib::SimpleClientGoalState::PREEMPTED:
00536 ROS_INFO_STREAM_NAMED("move_base",
00537 "The recovery action has been preempted!");
00538 if(action_state_ == CANCELED)
00539 goal_handle_.setCanceled();
00540 break;
00541
00542 case actionlib::SimpleClientGoalState::RECALLED:
00543 ROS_INFO_STREAM_NAMED("move_base",
00544 "The recovery action has been recalled!");
00545 if(action_state_ == CANCELED)
00546 goal_handle_.setCanceled();
00547 break;
00548
00549 case actionlib::SimpleClientGoalState::REJECTED:
00550 ROS_FATAL_STREAM_NAMED("move_base",
00551 "The recovery action has been rejected!");
00552 goal_handle_.setRejected();
00553 break;
00554 case actionlib::SimpleClientGoalState::LOST:
00555 ROS_FATAL_STREAM_NAMED("move_base",
00556 "The recovery action has lost the connection to the server!");
00557 goal_handle_.setAborted();
00558 break;
00559 default:
00560 ROS_FATAL_STREAM_NAMED("move_base",
00561 "Reached unreachable case! Unknown state!");
00562 goal_handle_.setAborted();
00563 break;
00564 }
00565 }
00566
00567 void MoveBaseAction::actionGetPathReplanningDone(
00568 const actionlib::SimpleClientGoalState &state,
00569 const mbf_msgs::GetPathResultConstPtr &result)
00570 {
00571 if (!replanning_ || action_state_ != EXE_PATH)
00572 return;
00573
00574 if(state == actionlib::SimpleClientGoalState::SUCCEEDED)
00575 {
00576 ROS_DEBUG_STREAM_NAMED("move_base", "Replanning succeeded; sending a goal to \"exe_path\" with the new plan");
00577 exe_path_goal_.path = result->path;
00578 mbf_msgs::ExePathGoal goal(exe_path_goal_);
00579 action_client_exe_path_.sendGoal(
00580 goal,
00581 boost::bind(&MoveBaseAction::actionExePathDone, this, _1, _2),
00582 boost::bind(&MoveBaseAction::actionExePathActive, this),
00583 boost::bind(&MoveBaseAction::actionExePathFeedback, this, _1));
00584 }
00585
00586 replanning_mtx_.lock();
00587 replanning_rate_.sleep();
00588 replanning_mtx_.unlock();
00589
00590 if (!replanning_ || action_state_ != EXE_PATH)
00591 return;
00592
00593 ROS_DEBUG_STREAM_NAMED("move_base", "Next replanning cycle, using the \"get_path\" action!");
00594 action_client_get_path_.sendGoal(
00595 get_path_goal_,
00596 boost::bind(&MoveBaseAction::actionGetPathReplanningDone, this, _1, _2));
00597 }
00598
00599
00600 }
00601