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