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 #include <move_base/move_base.h>
00039 #include <boost/algorithm/string.hpp>
00040
00041 namespace move_base {
00042
00043 MoveBase::MoveBase(std::string name, tf::TransformListener& tf) :
00044 tf_(tf),
00045 as_(NULL),
00046 tc_(NULL), planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),
00047 planner_(NULL), bgp_loader_("nav_core", "nav_core::BaseGlobalPlanner"),
00048 blp_loader_("nav_core", "nav_core::BaseLocalPlanner"),
00049 recovery_loader_("nav_core", "nav_core::RecoveryBehavior"),
00050 planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
00051 runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false) {
00052
00053 as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);
00054
00055 ros::NodeHandle private_nh("~");
00056 ros::NodeHandle nh;
00057
00058 recovery_trigger_ = PLANNING_R;
00059
00060
00061 std::string global_planner, local_planner;
00062 private_nh.param("base_global_planner", global_planner, std::string("navfn/NavfnROS"));
00063 private_nh.param("base_local_planner", local_planner, std::string("base_local_planner/TrajectoryPlannerROS"));
00064 private_nh.param("global_costmap/robot_base_frame", robot_base_frame_, std::string("base_link"));
00065 private_nh.param("global_costmap/global_frame", global_frame_, std::string("/map"));
00066 private_nh.param("planner_frequency", planner_frequency_, 0.0);
00067 private_nh.param("controller_frequency", controller_frequency_, 20.0);
00068 private_nh.param("planner_patience", planner_patience_, 5.0);
00069 private_nh.param("controller_patience", controller_patience_, 15.0);
00070
00071 private_nh.param("oscillation_timeout", oscillation_timeout_, 0.0);
00072 private_nh.param("oscillation_distance", oscillation_distance_, 0.5);
00073
00074
00075 planner_plan_ = new std::vector<geometry_msgs::PoseStamped>();
00076 latest_plan_ = new std::vector<geometry_msgs::PoseStamped>();
00077 controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();
00078
00079
00080 planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));
00081
00082
00083 vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
00084 current_goal_pub_ = private_nh.advertise<geometry_msgs::PoseStamped>("current_goal", 0 );
00085
00086 ros::NodeHandle action_nh("move_base");
00087 action_goal_pub_ = action_nh.advertise<move_base_msgs::MoveBaseActionGoal>("goal", 1);
00088
00089
00090
00091
00092 ros::NodeHandle simple_nh("move_base_simple");
00093 goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, boost::bind(&MoveBase::goalCB, this, _1));
00094
00095
00096 private_nh.param("local_costmap/inscribed_radius", inscribed_radius_, 0.325);
00097 private_nh.param("local_costmap/circumscribed_radius", circumscribed_radius_, 0.46);
00098 private_nh.param("clearing_radius", clearing_radius_, circumscribed_radius_);
00099 private_nh.param("conservative_reset_dist", conservative_reset_dist_, 3.0);
00100
00101 private_nh.param("shutdown_costmaps", shutdown_costmaps_, false);
00102 private_nh.param("clearing_roatation_allowed", clearing_roatation_allowed_, true);
00103 private_nh.param("recovery_behavior_enabled", recovery_behavior_enabled_, true);
00104
00105
00106 planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
00107 planner_costmap_ros_->pause();
00108
00109
00110 try {
00111
00112 if(!bgp_loader_.isClassAvailable(global_planner)){
00113 std::vector<std::string> classes = bgp_loader_.getDeclaredClasses();
00114 for(unsigned int i = 0; i < classes.size(); ++i){
00115 if(global_planner == bgp_loader_.getName(classes[i])){
00116
00117 ROS_WARN("Planner specifications should now include the package name. You are using a deprecated API. Please switch from %s to %s in your yaml file.",
00118 global_planner.c_str(), classes[i].c_str());
00119 global_planner = classes[i];
00120 break;
00121 }
00122 }
00123 }
00124
00125 planner_ = bgp_loader_.createClassInstance(global_planner);
00126 planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);
00127 } catch (const pluginlib::PluginlibException& ex)
00128 {
00129 ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", global_planner.c_str(), ex.what());
00130 exit(1);
00131 }
00132
00133 ROS_INFO("MAP SIZE: %d, %d", planner_costmap_ros_->getSizeInCellsX(), planner_costmap_ros_->getSizeInCellsY());
00134
00135
00136 controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
00137 controller_costmap_ros_->pause();
00138
00139
00140 try {
00141
00142 if(!blp_loader_.isClassAvailable(local_planner)){
00143 std::vector<std::string> classes = blp_loader_.getDeclaredClasses();
00144 for(unsigned int i = 0; i < classes.size(); ++i){
00145 if(local_planner == blp_loader_.getName(classes[i])){
00146
00147 ROS_WARN("Planner specifications should now include the package name. You are using a deprecated API. Please switch from %s to %s in your yaml file.",
00148 local_planner.c_str(), classes[i].c_str());
00149 local_planner = classes[i];
00150 break;
00151 }
00152 }
00153 }
00154
00155 tc_ = blp_loader_.createClassInstance(local_planner);
00156 tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);
00157 } catch (const pluginlib::PluginlibException& ex)
00158 {
00159 ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", local_planner.c_str(), ex.what());
00160 exit(1);
00161 }
00162
00163
00164 planner_costmap_ros_->start();
00165 controller_costmap_ros_->start();
00166
00167
00168 make_plan_srv_ = private_nh.advertiseService("make_plan", &MoveBase::planService, this);
00169
00170
00171 clear_unknown_srv_ = private_nh.advertiseService("clear_unknown_space", &MoveBase::clearUnknownService, this);
00172
00173
00174 clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this);
00175
00176
00177 planner_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 4, circumscribed_radius_ * 4);
00178 controller_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 4, circumscribed_radius_ * 4);
00179
00180
00181 if(shutdown_costmaps_){
00182 ROS_DEBUG_NAMED("move_base","Stopping costmaps initially");
00183 planner_costmap_ros_->stop();
00184 controller_costmap_ros_->stop();
00185 }
00186
00187
00188 if(!loadRecoveryBehaviors(private_nh)){
00189 loadDefaultRecoveryBehaviors();
00190 }
00191
00192
00193 state_ = PLANNING;
00194
00195
00196 recovery_index_ = 0;
00197
00198
00199 as_->start();
00200
00201 dsrv_ = new dynamic_reconfigure::Server<move_base::MoveBaseConfig>(ros::NodeHandle("~"));
00202 dynamic_reconfigure::Server<move_base::MoveBaseConfig>::CallbackType cb = boost::bind(&MoveBase::reconfigureCB, this, _1, _2);
00203 dsrv_->setCallback(cb);
00204 }
00205
00206 void MoveBase::reconfigureCB(move_base::MoveBaseConfig &config, uint32_t level){
00207 boost::recursive_mutex::scoped_lock l(configuration_mutex_);
00208
00209
00210
00211 if(!setup_)
00212 {
00213 last_config_ = config;
00214 default_config_ = config;
00215 setup_ = true;
00216 return;
00217 }
00218
00219 if(config.restore_defaults) {
00220 config = default_config_;
00221
00222 config.restore_defaults = false;
00223 }
00224
00225 if(planner_frequency_ != config.planner_frequency)
00226 {
00227 planner_frequency_ = config.planner_frequency;
00228 p_freq_change_ = true;
00229 }
00230
00231 if(controller_frequency_ != config.controller_frequency)
00232 {
00233 controller_frequency_ = config.controller_frequency;
00234 c_freq_change_ = true;
00235 }
00236
00237 planner_patience_ = config.planner_patience;
00238 controller_patience_ = config.controller_patience;
00239 conservative_reset_dist_ = config.conservative_reset_dist;
00240
00241 recovery_behavior_enabled_ = config.recovery_behavior_enabled;
00242 clearing_roatation_allowed_ = config.clearing_rotation_allowed;
00243 shutdown_costmaps_ = config.shutdown_costmaps;
00244
00245 oscillation_timeout_ = config.oscillation_timeout;
00246 oscillation_distance_ = config.oscillation_distance;
00247 if(config.base_global_planner != last_config_.base_global_planner) {
00248 nav_core::BaseGlobalPlanner* old_planner = planner_;
00249
00250 ROS_INFO("Loading global planner %s", config.base_global_planner.c_str());
00251 try {
00252
00253 if(!bgp_loader_.isClassAvailable(config.base_global_planner)){
00254 std::vector<std::string> classes = bgp_loader_.getDeclaredClasses();
00255 for(unsigned int i = 0; i < classes.size(); ++i){
00256 if(config.base_global_planner == bgp_loader_.getName(classes[i])){
00257
00258 ROS_WARN("Planner specifications should now include the package name. You are using a deprecated API. Please switch from %s to %s in your yaml file.",
00259 config.base_global_planner.c_str(), classes[i].c_str());
00260 config.base_global_planner = classes[i];
00261 break;
00262 }
00263 }
00264 }
00265
00266 planner_ = bgp_loader_.createClassInstance(config.base_global_planner);
00267
00268
00269 boost::unique_lock<boost::mutex> lock(planner_mutex_);
00270
00271 delete old_planner;
00272
00273 planner_plan_->clear();
00274 latest_plan_->clear();
00275 controller_plan_->clear();
00276 resetState();
00277 planner_->initialize(bgp_loader_.getName(config.base_global_planner), planner_costmap_ros_);
00278
00279 lock.unlock();
00280 } catch (const pluginlib::PluginlibException& ex)
00281 {
00282 ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", config.base_global_planner.c_str(), ex.what());
00283 planner_ = old_planner;
00284 config.base_global_planner = last_config_.base_global_planner;
00285 }
00286 }
00287
00288 if(config.base_local_planner != last_config_.base_local_planner){
00289 nav_core::BaseLocalPlanner* old_planner = tc_;
00290
00291 try {
00292
00293 ROS_INFO("Loading local planner: %s", config.base_local_planner.c_str());
00294 if(!blp_loader_.isClassAvailable(config.base_local_planner)){
00295 std::vector<std::string> classes = blp_loader_.getDeclaredClasses();
00296 for(unsigned int i = 0; i < classes.size(); ++i){
00297 if(config.base_local_planner == blp_loader_.getName(classes[i])){
00298
00299 ROS_WARN("Planner specifications should now include the package name. You are using a deprecated API. Please switch from %s to %s in your yaml file.",
00300 config.base_local_planner.c_str(), classes[i].c_str());
00301 config.base_local_planner = classes[i];
00302 break;
00303 }
00304 }
00305 }
00306 tc_ = blp_loader_.createClassInstance(config.base_local_planner);
00307 delete old_planner;
00308
00309 planner_plan_->clear();
00310 latest_plan_->clear();
00311 controller_plan_->clear();
00312 resetState();
00313 tc_->initialize(blp_loader_.getName(config.base_local_planner), &tf_, controller_costmap_ros_);
00314 } catch (const pluginlib::PluginlibException& ex)
00315 {
00316 ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", config.base_local_planner.c_str(), ex.what());
00317 tc_ = old_planner;
00318 config.base_local_planner = last_config_.base_local_planner;
00319 }
00320 }
00321
00322 last_config_ = config;
00323 }
00324
00325 void MoveBase::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal){
00326 ROS_DEBUG_NAMED("move_base","In ROS goal callback, wrapping the PoseStamped in the action message and re-sending to the server.");
00327 move_base_msgs::MoveBaseActionGoal action_goal;
00328 action_goal.header.stamp = ros::Time::now();
00329 action_goal.goal.target_pose = *goal;
00330
00331 action_goal_pub_.publish(action_goal);
00332 }
00333
00334 void MoveBase::clearCostmapWindows(double size_x, double size_y){
00335 tf::Stamped<tf::Pose> global_pose;
00336
00337
00338 planner_costmap_ros_->getRobotPose(global_pose);
00339
00340 std::vector<geometry_msgs::Point> clear_poly;
00341 double x = global_pose.getOrigin().x();
00342 double y = global_pose.getOrigin().y();
00343 geometry_msgs::Point pt;
00344
00345 pt.x = x - size_x / 2;
00346 pt.y = y - size_x / 2;
00347 clear_poly.push_back(pt);
00348
00349 pt.x = x + size_x / 2;
00350 pt.y = y - size_x / 2;
00351 clear_poly.push_back(pt);
00352
00353 pt.x = x + size_x / 2;
00354 pt.y = y + size_x / 2;
00355 clear_poly.push_back(pt);
00356
00357 pt.x = x - size_x / 2;
00358 pt.y = y + size_x / 2;
00359 clear_poly.push_back(pt);
00360
00361 planner_costmap_ros_->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE);
00362
00363
00364 controller_costmap_ros_->getRobotPose(global_pose);
00365
00366 clear_poly.clear();
00367 x = global_pose.getOrigin().x();
00368 y = global_pose.getOrigin().y();
00369
00370 pt.x = x - size_x / 2;
00371 pt.y = y - size_x / 2;
00372 clear_poly.push_back(pt);
00373
00374 pt.x = x + size_x / 2;
00375 pt.y = y - size_x / 2;
00376 clear_poly.push_back(pt);
00377
00378 pt.x = x + size_x / 2;
00379 pt.y = y + size_x / 2;
00380 clear_poly.push_back(pt);
00381
00382 pt.x = x - size_x / 2;
00383 pt.y = y + size_x / 2;
00384 clear_poly.push_back(pt);
00385
00386 controller_costmap_ros_->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE);
00387 }
00388
00389 bool MoveBase::clearUnknownService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp){
00390
00391 planner_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 4, circumscribed_radius_ * 4);
00392 controller_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 4, circumscribed_radius_ * 4);
00393 return true;
00394 }
00395
00396 bool MoveBase::clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp){
00397
00398 planner_costmap_ros_->resetMapOutsideWindow(0,0);
00399 controller_costmap_ros_->resetMapOutsideWindow(0,0);
00400 return true;
00401 }
00402
00403 bool MoveBase::planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp){
00404 if(as_->isActive()){
00405 ROS_ERROR("move_base must be in an inactive state to make a plan for an external user");
00406 return false;
00407 }
00408
00409
00410 if(planner_costmap_ros_ == NULL){
00411 ROS_ERROR("move_base cannot make a plan for you because it doesn't have a costmap");
00412 return false;
00413 }
00414
00415 tf::Stamped<tf::Pose> global_pose;
00416 if(!planner_costmap_ros_->getRobotPose(global_pose)){
00417 ROS_ERROR("move_base cannot make a plan for you because it could not get the start pose of the robot");
00418 return false;
00419 }
00420
00421 geometry_msgs::PoseStamped start;
00422
00423 if(req.start.header.frame_id == "")
00424 tf::poseStampedTFToMsg(global_pose, start);
00425 else
00426 start = req.start;
00427
00428
00429 clearCostmapWindows(2 * clearing_radius_, 2 * clearing_radius_);
00430
00431
00432
00433 double resolution = planner_costmap_ros_->getResolution();
00434 std::vector<geometry_msgs::PoseStamped> global_plan;
00435 geometry_msgs::PoseStamped p;
00436 p = req.goal;
00437 p.pose.position.y = req.goal.pose.position.y - req.tolerance;
00438 bool found_legal = false;
00439 while(!found_legal && p.pose.position.y <= req.goal.pose.position.y + req.tolerance){
00440 p.pose.position.x = req.goal.pose.position.x - req.tolerance;
00441 while(!found_legal && p.pose.position.x <= req.goal.pose.position.x + req.tolerance){
00442 if(planner_->makePlan(start, p, global_plan)){
00443 if(!global_plan.empty()){
00444 global_plan.push_back(p);
00445 found_legal = true;
00446 }
00447 else
00448 ROS_DEBUG_NAMED("move_base","Failed to find a plan to point (%.2f, %.2f)", p.pose.position.x, p.pose.position.y);
00449 }
00450 p.pose.position.x += resolution*3.0;
00451 }
00452 p.pose.position.y += resolution*3.0;
00453 }
00454
00455
00456 resp.plan.poses.resize(global_plan.size());
00457 for(unsigned int i = 0; i < global_plan.size(); ++i){
00458 resp.plan.poses[i] = global_plan[i];
00459 }
00460
00461
00462
00463 return true;
00464 }
00465
00466 MoveBase::~MoveBase(){
00467 recovery_behaviors_.clear();
00468
00469 delete dsrv_;
00470
00471 if(as_ != NULL)
00472 delete as_;
00473
00474 if(planner_ != NULL)
00475 delete planner_;
00476
00477 if(tc_ != NULL)
00478 delete tc_;
00479
00480 if(planner_costmap_ros_ != NULL)
00481 delete planner_costmap_ros_;
00482
00483 if(controller_costmap_ros_ != NULL)
00484 delete controller_costmap_ros_;
00485
00486 planner_thread_->interrupt();
00487 planner_thread_->join();
00488
00489 delete planner_plan_;
00490 delete latest_plan_;
00491 delete controller_plan_;
00492 }
00493
00494 bool MoveBase::makePlan(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan){
00495
00496 plan.clear();
00497
00498
00499 if(planner_costmap_ros_ == NULL) {
00500 ROS_ERROR("Planner costmap ROS is NULL, unable to create global plan");
00501 return false;
00502 }
00503
00504
00505 tf::Stamped<tf::Pose> global_pose;
00506 if(!planner_costmap_ros_->getRobotPose(global_pose)) {
00507 ROS_WARN("Unable to get starting pose of robot, unable to create global plan");
00508 return false;
00509 }
00510
00511 geometry_msgs::PoseStamped start;
00512 tf::poseStampedTFToMsg(global_pose, start);
00513
00514
00515 if(!planner_->makePlan(start, goal, plan) || plan.empty()){
00516 ROS_DEBUG_NAMED("move_base","Failed to find a plan to point (%.2f, %.2f)", goal.pose.position.x, goal.pose.position.y);
00517 return false;
00518 }
00519
00520 return true;
00521 }
00522
00523 void MoveBase::publishZeroVelocity(){
00524 geometry_msgs::Twist cmd_vel;
00525 cmd_vel.linear.x = 0.0;
00526 cmd_vel.linear.y = 0.0;
00527 cmd_vel.angular.z = 0.0;
00528 vel_pub_.publish(cmd_vel);
00529
00530 }
00531
00532 bool MoveBase::isQuaternionValid(const geometry_msgs::Quaternion& q){
00533
00534 if(!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w)){
00535 ROS_ERROR("Quaternion has nans or infs... discarding as a navigation goal");
00536 return false;
00537 }
00538
00539 tf::Quaternion tf_q(q.x, q.y, q.z, q.w);
00540
00541
00542 if(tf_q.length2() < 1e-6){
00543 ROS_ERROR("Quaternion has length close to zero... discarding as navigation goal");
00544 return false;
00545 }
00546
00547
00548 tf_q.normalize();
00549
00550 tf::Vector3 up(0, 0, 1);
00551
00552 double dot = up.dot(up.rotate(tf_q.getAxis(), tf_q.getAngle()));
00553
00554 if(fabs(dot - 1) > 1e-3){
00555 ROS_ERROR("Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical.");
00556 return false;
00557 }
00558
00559 return true;
00560 }
00561
00562 geometry_msgs::PoseStamped MoveBase::goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg){
00563 std::string global_frame = planner_costmap_ros_->getGlobalFrameID();
00564 tf::Stamped<tf::Pose> goal_pose, global_pose;
00565 poseStampedMsgToTF(goal_pose_msg, goal_pose);
00566
00567
00568
00569 goal_pose.stamp_ = ros::Time();
00570
00571 try{
00572 tf_.transformPose(global_frame, goal_pose, global_pose);
00573 }
00574 catch(tf::TransformException& ex){
00575 ROS_WARN("Failed to transform the goal pose from %s into the %s frame: %s",
00576 goal_pose.frame_id_.c_str(), global_frame.c_str(), ex.what());
00577 return goal_pose_msg;
00578 }
00579
00580 geometry_msgs::PoseStamped global_pose_msg;
00581 tf::poseStampedTFToMsg(global_pose, global_pose_msg);
00582 return global_pose_msg;
00583
00584 }
00585
00586 void MoveBase::planThread(){
00587 ROS_DEBUG_NAMED("move_base_plan_thread","Starting planner thread...");
00588 ros::NodeHandle n;
00589 ros::Rate r(planner_frequency_);
00590 boost::unique_lock<boost::mutex> lock(planner_mutex_);
00591 while(n.ok()){
00592 if(p_freq_change_)
00593 {
00594 ROS_INFO("Setting planner frequency to %.2f", planner_frequency_);
00595 r = ros::Rate(planner_frequency_);
00596 p_freq_change_ = false;
00597 }
00598
00599
00600 while(!runPlanner_){
00601
00602 ROS_DEBUG_NAMED("move_base_plan_thread","Planner thread is suspending");
00603 planner_cond_.wait(lock);
00604 }
00605
00606 geometry_msgs::PoseStamped temp_goal = planner_goal_;
00607 lock.unlock();
00608 ROS_DEBUG_NAMED("move_base_plan_thread","Planning...");
00609
00610
00611 planner_plan_->clear();
00612 bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_);
00613
00614 if(gotPlan){
00615 ROS_DEBUG_NAMED("move_base_plan_thread","Got Plan with %zu points!", planner_plan_->size());
00616
00617 std::vector<geometry_msgs::PoseStamped>* temp_plan = planner_plan_;
00618
00619 lock.lock();
00620 planner_plan_ = latest_plan_;
00621 latest_plan_ = temp_plan;
00622 last_valid_plan_ = ros::Time::now();
00623 new_global_plan_ = true;
00624
00625 ROS_DEBUG_NAMED("move_base_plan_thread","Generated a plan from the base_global_planner");
00626
00627
00628 if(runPlanner_)
00629 state_ = CONTROLLING;
00630 if(planner_frequency_ <= 0)
00631 runPlanner_ = false;
00632 lock.unlock();
00633 }
00634
00635 else if(state_==PLANNING){
00636 ROS_DEBUG_NAMED("move_base_plan_thread","No Plan...");
00637 ros::Time attempt_end = last_valid_plan_ + ros::Duration(planner_patience_);
00638
00639
00640 if(ros::Time::now() > attempt_end){
00641
00642 state_ = CLEARING;
00643 publishZeroVelocity();
00644 recovery_trigger_ = PLANNING_R;
00645 }
00646 }
00647
00648 if(!p_freq_change_ && planner_frequency_ > 0)
00649 r.sleep();
00650
00651
00652 lock.lock();
00653 }
00654 }
00655
00656 void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal)
00657 {
00658 if(!isQuaternionValid(move_base_goal->target_pose.pose.orientation)){
00659 as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
00660 return;
00661 }
00662
00663 geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose);
00664
00665
00666 boost::unique_lock<boost::mutex> lock(planner_mutex_);
00667 planner_goal_ = goal;
00668 runPlanner_ = true;
00669 planner_cond_.notify_one();
00670 lock.unlock();
00671
00672 current_goal_pub_.publish(goal);
00673 std::vector<geometry_msgs::PoseStamped> global_plan;
00674
00675 ros::Rate r(controller_frequency_);
00676 if(shutdown_costmaps_){
00677 ROS_DEBUG_NAMED("move_base","Starting up costmaps that were shut down previously");
00678 planner_costmap_ros_->start();
00679 controller_costmap_ros_->start();
00680 }
00681
00682
00683 last_valid_control_ = ros::Time::now();
00684 last_valid_plan_ = ros::Time::now();
00685 last_oscillation_reset_ = ros::Time::now();
00686
00687 ros::NodeHandle n;
00688 while(n.ok())
00689 {
00690 if(c_freq_change_)
00691 {
00692 ROS_INFO("Setting controller frequency to %.2f", controller_frequency_);
00693 r = ros::Rate(controller_frequency_);
00694 c_freq_change_ = false;
00695 }
00696
00697 if(as_->isPreemptRequested()){
00698 if(as_->isNewGoalAvailable()){
00699
00700 move_base_msgs::MoveBaseGoal new_goal = *as_->acceptNewGoal();
00701
00702 if(!isQuaternionValid(new_goal.target_pose.pose.orientation)){
00703 as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
00704 return;
00705 }
00706
00707 goal = goalToGlobalFrame(new_goal.target_pose);
00708
00709
00710 recovery_index_ = 0;
00711 state_ = PLANNING;
00712
00713
00714 lock.lock();
00715 planner_goal_ = goal;
00716 runPlanner_ = true;
00717 planner_cond_.notify_one();
00718 lock.unlock();
00719
00720
00721 ROS_DEBUG_NAMED("move_base","move_base has received a goal of x: %.2f, y: %.2f", goal.pose.position.x, goal.pose.position.y);
00722 current_goal_pub_.publish(goal);
00723
00724
00725 last_valid_control_ = ros::Time::now();
00726 last_valid_plan_ = ros::Time::now();
00727 last_oscillation_reset_ = ros::Time::now();
00728 }
00729 else {
00730
00731 resetState();
00732
00733
00734 ROS_DEBUG_NAMED("move_base","Move base preempting the current goal");
00735 as_->setPreempted();
00736
00737
00738 return;
00739 }
00740 }
00741
00742
00743 if(goal.header.frame_id != planner_costmap_ros_->getGlobalFrameID()){
00744 goal = goalToGlobalFrame(goal);
00745
00746
00747 recovery_index_ = 0;
00748 state_ = PLANNING;
00749
00750
00751 lock.lock();
00752 planner_goal_ = goal;
00753 runPlanner_ = true;
00754 planner_cond_.notify_one();
00755 lock.unlock();
00756
00757
00758 ROS_DEBUG_NAMED("move_base","The global frame for move_base has changed, new frame: %s, new goal position x: %.2f, y: %.2f", goal.header.frame_id.c_str(), goal.pose.position.x, goal.pose.position.y);
00759 current_goal_pub_.publish(goal);
00760
00761
00762 last_valid_control_ = ros::Time::now();
00763 last_valid_plan_ = ros::Time::now();
00764 last_oscillation_reset_ = ros::Time::now();
00765 }
00766
00767
00768 ros::WallTime start = ros::WallTime::now();
00769
00770
00771 bool done = executeCycle(goal, global_plan);
00772
00773
00774 if(done)
00775 return;
00776
00777
00778
00779 ros::WallDuration t_diff = ros::WallTime::now() - start;
00780 ROS_DEBUG_NAMED("move_base","Full control cycle time: %.9f\n", t_diff.toSec());
00781
00782 r.sleep();
00783
00784 if(r.cycleTime() > ros::Duration(1 / controller_frequency_) && state_ == CONTROLLING)
00785 ROS_WARN("Control loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", controller_frequency_, r.cycleTime().toSec());
00786 }
00787
00788
00789 lock.lock();
00790 runPlanner_ = true;
00791 planner_cond_.notify_one();
00792 lock.unlock();
00793
00794
00795 as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on the goal because the node has been killed");
00796 return;
00797 }
00798
00799 double MoveBase::distance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2)
00800 {
00801 return sqrt((p1.pose.position.x - p2.pose.position.x) * (p1.pose.position.x - p2.pose.position.x)
00802 + (p1.pose.position.y - p2.pose.position.y) * (p1.pose.position.y - p2.pose.position.y));
00803 }
00804
00805 bool MoveBase::executeCycle(geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& global_plan){
00806 boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
00807
00808 geometry_msgs::Twist cmd_vel;
00809
00810
00811 tf::Stamped<tf::Pose> global_pose;
00812 planner_costmap_ros_->getRobotPose(global_pose);
00813 geometry_msgs::PoseStamped current_position;
00814 tf::poseStampedTFToMsg(global_pose, current_position);
00815
00816
00817 move_base_msgs::MoveBaseFeedback feedback;
00818 feedback.base_position = current_position;
00819 as_->publishFeedback(feedback);
00820
00821
00822 if(distance(current_position, oscillation_pose_) >= oscillation_distance_)
00823 {
00824 last_oscillation_reset_ = ros::Time::now();
00825 oscillation_pose_ = current_position;
00826
00827
00828 if(recovery_trigger_ == OSCILLATION_R)
00829 recovery_index_ = 0;
00830 }
00831
00832
00833 if(!controller_costmap_ros_->isCurrent()){
00834 ROS_WARN("[%s]:Sensor data is out of date, we're not going to allow commanding of the base for safety",ros::this_node::getName().c_str());
00835 publishZeroVelocity();
00836 return false;
00837 }
00838
00839
00840 if(new_global_plan_){
00841
00842 new_global_plan_ = false;
00843
00844 ROS_DEBUG_NAMED("move_base","Got a new plan...swap pointers");
00845
00846
00847 std::vector<geometry_msgs::PoseStamped>* temp_plan = controller_plan_;
00848
00849 boost::unique_lock<boost::mutex> lock(planner_mutex_);
00850 controller_plan_ = latest_plan_;
00851 latest_plan_ = temp_plan;
00852 lock.unlock();
00853 ROS_DEBUG_NAMED("move_base","pointers swapped!");
00854
00855 if(!tc_->setPlan(*controller_plan_)){
00856
00857 ROS_ERROR("Failed to pass global plan to the controller, aborting.");
00858 resetState();
00859
00860
00861 lock.lock();
00862 runPlanner_ = false;
00863 lock.unlock();
00864
00865 as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to pass global plan to the controller.");
00866 return true;
00867 }
00868
00869
00870 if(recovery_trigger_ == PLANNING_R)
00871 recovery_index_ = 0;
00872 }
00873
00874
00875 switch(state_){
00876
00877 case PLANNING:
00878 {
00879 boost::mutex::scoped_lock lock(planner_mutex_);
00880 runPlanner_ = true;
00881 planner_cond_.notify_one();
00882 }
00883 ROS_DEBUG_NAMED("move_base","Waiting for plan, in the planning state.");
00884 break;
00885
00886
00887 case CONTROLLING:
00888 ROS_DEBUG_NAMED("move_base","In controlling state.");
00889
00890
00891 if(tc_->isGoalReached()){
00892 ROS_DEBUG_NAMED("move_base","Goal reached!");
00893 resetState();
00894
00895
00896 boost::unique_lock<boost::mutex> lock(planner_mutex_);
00897 runPlanner_ = false;
00898 lock.unlock();
00899
00900 as_->setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached.");
00901 return true;
00902 }
00903
00904
00905 if(oscillation_timeout_ > 0.0 &&
00906 last_oscillation_reset_ + ros::Duration(oscillation_timeout_) < ros::Time::now())
00907 {
00908 publishZeroVelocity();
00909 state_ = CLEARING;
00910 recovery_trigger_ = OSCILLATION_R;
00911 }
00912
00913 if(tc_->computeVelocityCommands(cmd_vel)){
00914 ROS_DEBUG_NAMED("move_base", "Got a valid command from the local planner.");
00915 last_valid_control_ = ros::Time::now();
00916
00917 vel_pub_.publish(cmd_vel);
00918 if(recovery_trigger_ == CONTROLLING_R)
00919 recovery_index_ = 0;
00920 }
00921 else {
00922 ROS_DEBUG_NAMED("move_base", "The local planner could not find a valid plan.");
00923 ros::Time attempt_end = last_valid_control_ + ros::Duration(controller_patience_);
00924
00925
00926 if(ros::Time::now() > attempt_end){
00927
00928 publishZeroVelocity();
00929 state_ = CLEARING;
00930 recovery_trigger_ = CONTROLLING_R;
00931 }
00932 else{
00933
00934 last_valid_plan_ = ros::Time::now();
00935 state_ = PLANNING;
00936 publishZeroVelocity();
00937
00938
00939 boost::unique_lock<boost::mutex> lock(planner_mutex_);
00940 runPlanner_ = true;
00941 planner_cond_.notify_one();
00942 lock.unlock();
00943 }
00944 }
00945
00946 break;
00947
00948
00949 case CLEARING:
00950 ROS_DEBUG_NAMED("move_base","In clearing/recovery state");
00951
00952 if(recovery_behavior_enabled_ && recovery_index_ < recovery_behaviors_.size()){
00953 ROS_DEBUG_NAMED("move_base_recovery","Executing behavior %u of %zu", recovery_index_, recovery_behaviors_.size());
00954 recovery_behaviors_[recovery_index_]->runBehavior();
00955
00956
00957 last_oscillation_reset_ = ros::Time::now();
00958
00959
00960 ROS_DEBUG_NAMED("move_base_recovery","Going back to planning state");
00961 state_ = PLANNING;
00962
00963
00964 recovery_index_++;
00965 }
00966 else{
00967 ROS_DEBUG_NAMED("move_base_recovery","All recovery behaviors have failed, locking the planner and disabling it.");
00968
00969 boost::unique_lock<boost::mutex> lock(planner_mutex_);
00970 runPlanner_ = false;
00971 lock.unlock();
00972
00973 ROS_DEBUG_NAMED("move_base_recovery","Something should abort after this.");
00974
00975 if(recovery_trigger_ == CONTROLLING_R){
00976 ROS_ERROR("Aborting because a valid control could not be found. Even after executing all recovery behaviors");
00977 as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid control. Even after executing recovery behaviors.");
00978 }
00979 else if(recovery_trigger_ == PLANNING_R){
00980 ROS_ERROR("Aborting because a valid plan could not be found. Even after executing all recovery behaviors");
00981 as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid plan. Even after executing recovery behaviors.");
00982 }
00983 else if(recovery_trigger_ == OSCILLATION_R){
00984 ROS_ERROR("Aborting because the robot appears to be oscillating over and over. Even after executing all recovery behaviors");
00985 as_->setAborted(move_base_msgs::MoveBaseResult(), "Robot is oscillating. Even after executing recovery behaviors.");
00986 }
00987 resetState();
00988 return true;
00989 }
00990 break;
00991 default:
00992 ROS_ERROR("This case should never be reached, something is wrong, aborting");
00993 resetState();
00994
00995 boost::unique_lock<boost::mutex> lock(planner_mutex_);
00996 runPlanner_ = false;
00997 lock.unlock();
00998 as_->setAborted(move_base_msgs::MoveBaseResult(), "Reached a case that should not be hit in move_base. This is a bug, please report it.");
00999 return true;
01000 }
01001
01002
01003 return false;
01004 }
01005
01006 bool MoveBase::loadRecoveryBehaviors(ros::NodeHandle node){
01007 XmlRpc::XmlRpcValue behavior_list;
01008 if(node.getParam("recovery_behaviors", behavior_list)){
01009 if(behavior_list.getType() == XmlRpc::XmlRpcValue::TypeArray){
01010 for(int i = 0; i < behavior_list.size(); ++i){
01011 if(behavior_list[i].getType() == XmlRpc::XmlRpcValue::TypeStruct){
01012 if(behavior_list[i].hasMember("name") && behavior_list[i].hasMember("type")){
01013
01014 for(int j = i + 1; j < behavior_list.size(); j++){
01015 if(behavior_list[j].getType() == XmlRpc::XmlRpcValue::TypeStruct){
01016 if(behavior_list[j].hasMember("name") && behavior_list[j].hasMember("type")){
01017 std::string name_i = behavior_list[i]["name"];
01018 std::string name_j = behavior_list[j]["name"];
01019 if(name_i == name_j){
01020 ROS_ERROR("A recovery behavior with the name %s already exists, this is not allowed. Using the default recovery behaviors instead.",
01021 name_i.c_str());
01022 return false;
01023 }
01024 }
01025 }
01026 }
01027 }
01028 else{
01029 ROS_ERROR("Recovery behaviors must have a name and a type and this does not. Using the default recovery behaviors instead.");
01030 return false;
01031 }
01032 }
01033 else{
01034 ROS_ERROR("Recovery behaviors must be specified as maps, but they are XmlRpcType %d. We'll use the default recovery behaviors instead.",
01035 behavior_list[i].getType());
01036 return false;
01037 }
01038 }
01039
01040
01041 for(int i = 0; i < behavior_list.size(); ++i){
01042 try{
01043
01044 if(!recovery_loader_.isClassAvailable(behavior_list[i]["type"])){
01045 std::vector<std::string> classes = recovery_loader_.getDeclaredClasses();
01046 for(unsigned int i = 0; i < classes.size(); ++i){
01047 if(behavior_list[i]["type"] == recovery_loader_.getName(classes[i])){
01048
01049 ROS_WARN("Recovery behavior specifications should now include the package name. You are using a deprecated API. Please switch from %s to %s in your yaml file.",
01050 std::string(behavior_list[i]["type"]).c_str(), classes[i].c_str());
01051 behavior_list[i]["type"] = classes[i];
01052 break;
01053 }
01054 }
01055 }
01056
01057 boost::shared_ptr<nav_core::RecoveryBehavior> behavior(recovery_loader_.createClassInstance(behavior_list[i]["type"]));
01058
01059
01060 if(behavior.get() == NULL){
01061 ROS_ERROR("The ClassLoader returned a null pointer without throwing an exception. This should not happen");
01062 return false;
01063 }
01064
01065
01066 behavior->initialize(behavior_list[i]["name"], &tf_, planner_costmap_ros_, controller_costmap_ros_);
01067 recovery_behaviors_.push_back(behavior);
01068 }
01069 catch(pluginlib::PluginlibException& ex){
01070 ROS_ERROR("Failed to load a plugin. Using default recovery behaviors. Error: %s", ex.what());
01071 return false;
01072 }
01073 }
01074 }
01075 else{
01076 ROS_ERROR("The recovery behavior specification must be a list, but is of XmlRpcType %d. We'll use the default recovery behaviors instead.",
01077 behavior_list.getType());
01078 return false;
01079 }
01080 }
01081 else{
01082
01083 return false;
01084 }
01085
01086
01087 return true;
01088 }
01089
01090
01091 void MoveBase::loadDefaultRecoveryBehaviors(){
01092 recovery_behaviors_.clear();
01093 try{
01094
01095 ros::NodeHandle n("~");
01096 n.setParam("conservative_reset/reset_distance", conservative_reset_dist_);
01097 n.setParam("aggressive_reset/reset_distance", circumscribed_radius_ * 4);
01098
01099
01100 boost::shared_ptr<nav_core::RecoveryBehavior> cons_clear(recovery_loader_.createClassInstance("clear_costmap_recovery/ClearCostmapRecovery"));
01101 cons_clear->initialize("conservative_reset", &tf_, planner_costmap_ros_, controller_costmap_ros_);
01102 recovery_behaviors_.push_back(cons_clear);
01103
01104
01105 boost::shared_ptr<nav_core::RecoveryBehavior> rotate(recovery_loader_.createClassInstance("rotate_recovery/RotateRecovery"));
01106 if(clearing_roatation_allowed_){
01107 rotate->initialize("rotate_recovery", &tf_, planner_costmap_ros_, controller_costmap_ros_);
01108 recovery_behaviors_.push_back(rotate);
01109 }
01110
01111
01112 boost::shared_ptr<nav_core::RecoveryBehavior> ags_clear(recovery_loader_.createClassInstance("clear_costmap_recovery/ClearCostmapRecovery"));
01113 ags_clear->initialize("aggressive_reset", &tf_, planner_costmap_ros_, controller_costmap_ros_);
01114 recovery_behaviors_.push_back(ags_clear);
01115
01116
01117 if(clearing_roatation_allowed_)
01118 recovery_behaviors_.push_back(rotate);
01119 }
01120 catch(pluginlib::PluginlibException& ex){
01121 ROS_FATAL("Failed to load a plugin. This should not happen on default recovery behaviors. Error: %s", ex.what());
01122 }
01123
01124 return;
01125 }
01126
01127 void MoveBase::resetState(){
01128 state_ = PLANNING;
01129 recovery_index_ = 0;
01130 recovery_trigger_ = PLANNING_R;
01131 publishZeroVelocity();
01132
01133
01134 if(shutdown_costmaps_){
01135 ROS_DEBUG_NAMED("move_base","Stopping costmaps");
01136 planner_costmap_ros_->stop();
01137 controller_costmap_ros_->stop();
01138 }
01139 }
01140
01141 };
01142
01143 int main(int argc, char** argv){
01144 ros::init(argc, argv, "move_base_node");
01145 tf::TransformListener tf(ros::Duration(10));
01146
01147 move_base::MoveBase move_base("move_base", tf);
01148
01149
01150 ros::spin();
01151
01152 return(0);
01153
01154 }