move_base.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2008, Willow Garage, Inc.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of the Willow Garage nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *
00035 * Author: Eitan Marder-Eppstein
00036 *         Mike Phillips (put the planner in its own thread)
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     //get some parameters that will be global to the move base node
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     //set up plan triple buffer
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     //set up the planner's thread
00080     planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));
00081 
00082     //for comanding the base
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     //we'll provide a mechanism for some people to send goals as PoseStamped messages over a topic
00090     //they won't get any useful information back about its status, but this is useful for tools
00091     //like nav_view and rviz
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     //we'll assume the radius of the robot to be consistent with what's specified for the costmaps
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     //create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
00106     planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
00107     planner_costmap_ros_->pause();
00108 
00109     //initialize the global planner
00110     try {
00111       //check if a non fully qualified name has potentially been passed in
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             //if we've found a match... we'll get the fully qualified name and break out of the loop
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     //create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
00136     controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
00137     controller_costmap_ros_->pause();
00138 
00139     //create a local planner
00140     try {
00141       //check if a non fully qualified name has potentially been passed in
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             //if we've found a match... we'll get the fully qualified name and break out of the loop
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     // Start actively updating costmaps based on sensor data
00164     planner_costmap_ros_->start();
00165     controller_costmap_ros_->start();
00166 
00167     //advertise a service for getting a plan
00168     make_plan_srv_ = private_nh.advertiseService("make_plan", &MoveBase::planService, this);
00169 
00170     //advertise a service for clearing the costmaps
00171     clear_unknown_srv_ = private_nh.advertiseService("clear_unknown_space", &MoveBase::clearUnknownService, this);
00172 
00173     //advertise a service for clearing the costmaps
00174     clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this);
00175 
00176     //initially clear any unknown space around the robot
00177     planner_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 4, circumscribed_radius_ * 4);
00178     controller_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 4, circumscribed_radius_ * 4);
00179 
00180     //if we shutdown our costmaps when we're deactivated... we'll do that now
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     //load any user specified recovery behaviors, and if that fails load the defaults
00188     if(!loadRecoveryBehaviors(private_nh)){
00189       loadDefaultRecoveryBehaviors();
00190     }
00191 
00192     //initially, we'll need to make a plan
00193     state_ = PLANNING;
00194 
00195     //we'll start executing recovery behaviors at the beginning of our list
00196     recovery_index_ = 0;
00197 
00198     //we're all set up now so we can start the action server
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     //The first time we're called, we just want to make sure we have the
00210     //original configuration
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       //if someone sets restore defaults on the parameter server, prevent looping
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       //initialize the global planner
00250       ROS_INFO("Loading global planner %s", config.base_global_planner.c_str());
00251       try {
00252         //check if a non fully qualified name has potentially been passed in
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               //if we've found a match... we'll get the fully qualified name and break out of the loop
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         // wait for the current planner to finish planning
00269         boost::unique_lock<boost::mutex> lock(planner_mutex_);
00270 
00271         delete old_planner;
00272         // Clean up before initializing the new planner
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       //create a local planner
00291       try {
00292         //check if a non fully qualified name has potentially been passed in
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               //if we've found a match... we'll get the fully qualified name and break out of the loop
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         // Clean up before initializing the new planner
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     //clear the planner's costmap
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     //clear the controller's costmap
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     //clear any unknown space around the robot the same as we do on initialization
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     //clear the costmaps
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     //make sure we have a costmap for our planner
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     //if the user does not specify a start pose, identified by an empty frame id, then use the robot's pose
00423     if(req.start.header.frame_id == "")
00424       tf::poseStampedTFToMsg(global_pose, start);
00425     else
00426       start = req.start;
00427 
00428     //update the copy of the costmap the planner uses
00429     clearCostmapWindows(2 * clearing_radius_, 2 * clearing_radius_);
00430 
00431     //if we have a tolerance on the goal point that is greater
00432     //than the resolution of the map... compute the full potential function
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     //copy the plan into a message to send out
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     //make sure to set the plan to be empty initially
00496     plan.clear();
00497 
00498     //since this gets called on handle activate
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     //get the starting pose of the robot
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     //if the planner fails or returns a zero length plan, planning failed
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     //first we need to check if the quaternion has nan's or infs
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     //next, we need to check if the length of the quaternion is close to zero
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     //next, we'll normalize the quaternion and check that it transforms the vertical vector correctly
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     //just get the latest available transform... for accuracy they should send
00568     //goals in the frame of the planner
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       //check if we should run the planner (the mutex is locked)
00600       while(!runPlanner_){
00601         //if we should not be running the planner then suspend this thread
00602         ROS_DEBUG_NAMED("move_base_plan_thread","Planner thread is suspending");
00603         planner_cond_.wait(lock);
00604       }
00605       //time to plan! get a copy of the goal and unlock the mutex
00606       geometry_msgs::PoseStamped temp_goal = planner_goal_;
00607       lock.unlock();
00608       ROS_DEBUG_NAMED("move_base_plan_thread","Planning...");
00609 
00610       //run planner
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         //pointer swap the plans under mutex (the controller will pull from latest_plan_)
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         //make sure we only start the controller if we still haven't reached the goal
00628         if(runPlanner_)
00629           state_ = CONTROLLING;
00630         if(planner_frequency_ <= 0)
00631           runPlanner_ = false;
00632         lock.unlock();
00633       }
00634       //if we didn't get a plan and we are in the planning state (the robot isn't moving)
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         //check if we've tried to make a plan for over our time limit
00640         if(ros::Time::now() > attempt_end){
00641           //we'll move into our obstacle clearing mode
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       //take the mutex for the next iteration
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     //we have a goal so start the planner
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     //we want to make sure that we reset the last time we had a valid plan and control
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           //if we're active and a new goal is available, we'll accept it, but we won't shut anything down
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           //we'll make sure that we reset our state for the next execution cycle
00710           recovery_index_ = 0;
00711           state_ = PLANNING;
00712 
00713           //we have a new goal so make sure the planner is awake
00714           lock.lock();
00715           planner_goal_ = goal;
00716           runPlanner_ = true;
00717           planner_cond_.notify_one();
00718           lock.unlock();
00719 
00720           //publish the goal point to the visualizer
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           //make sure to reset our timeouts
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           //if we've been preempted explicitly we need to shut things down
00731           resetState();
00732 
00733           //notify the ActionServer that we've successfully preempted
00734           ROS_DEBUG_NAMED("move_base","Move base preempting the current goal");
00735           as_->setPreempted();
00736 
00737           //we'll actually return from execute after preempting
00738           return;
00739         }
00740       }
00741 
00742       //we also want to check if we've changed global frames because we need to transform our goal pose
00743       if(goal.header.frame_id != planner_costmap_ros_->getGlobalFrameID()){
00744         goal = goalToGlobalFrame(goal);
00745 
00746         //we want to go back to the planning state for the next execution cycle
00747         recovery_index_ = 0;
00748         state_ = PLANNING;
00749 
00750         //we have a new goal so make sure the planner is awake
00751         lock.lock();
00752         planner_goal_ = goal;
00753         runPlanner_ = true;
00754         planner_cond_.notify_one();
00755         lock.unlock();
00756 
00757         //publish the goal point to the visualizer
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         //make sure to reset our timeouts
00762         last_valid_control_ = ros::Time::now();
00763         last_valid_plan_ = ros::Time::now();
00764         last_oscillation_reset_ = ros::Time::now();
00765       }
00766 
00767       //for timing that gives real time even in simulation
00768       ros::WallTime start = ros::WallTime::now();
00769 
00770       //the real work on pursuing a goal is done here
00771       bool done = executeCycle(goal, global_plan);
00772 
00773       //if we're done, then we'll return from execute
00774       if(done)
00775         return;
00776 
00777       //check if execution of the goal has completed in some way
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       //make sure to sleep for the remainder of our cycle time
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     //wake up the planner thread so that it can exit cleanly
00789     lock.lock();
00790     runPlanner_ = true;
00791     planner_cond_.notify_one();
00792     lock.unlock();
00793 
00794     //if the node is killed then we'll abort and return
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     //we need to be able to publish velocity commands
00808     geometry_msgs::Twist cmd_vel;
00809 
00810     //update feedback to correspond to our curent position
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     //push the feedback out
00817     move_base_msgs::MoveBaseFeedback feedback;
00818     feedback.base_position = current_position;
00819     as_->publishFeedback(feedback);
00820 
00821     //check to see if we've moved far enough to reset our oscillation timeout
00822     if(distance(current_position, oscillation_pose_) >= oscillation_distance_)
00823     {
00824       last_oscillation_reset_ = ros::Time::now();
00825       oscillation_pose_ = current_position;
00826 
00827       //if our last recovery was caused by oscillation, we want to reset the recovery index 
00828       if(recovery_trigger_ == OSCILLATION_R)
00829         recovery_index_ = 0;
00830     }
00831 
00832     //check that the observation buffers for the costmap are current, we don't want to drive blind
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     //if we have a new plan then grab it and give it to the controller
00840     if(new_global_plan_){
00841       //make sure to set the new plan flag to false
00842       new_global_plan_ = false;
00843 
00844       ROS_DEBUG_NAMED("move_base","Got a new plan...swap pointers");
00845 
00846       //do a pointer swap under mutex
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         //ABORT and SHUTDOWN COSTMAPS
00857         ROS_ERROR("Failed to pass global plan to the controller, aborting.");
00858         resetState();
00859 
00860         //disable the planner thread
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       //make sure to reset recovery_index_ since we were able to find a valid plan
00870       if(recovery_trigger_ == PLANNING_R)
00871         recovery_index_ = 0;
00872     }
00873 
00874     //the move_base state machine, handles the control logic for navigation
00875     switch(state_){
00876       //if we are in a planning state, then we'll attempt to make a plan
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       //if we're controlling, we'll attempt to find valid velocity commands
00887       case CONTROLLING:
00888         ROS_DEBUG_NAMED("move_base","In controlling state.");
00889 
00890         //check to see if we've reached our goal
00891         if(tc_->isGoalReached()){
00892           ROS_DEBUG_NAMED("move_base","Goal reached!");
00893           resetState();
00894 
00895           //disable the planner thread
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         //check for an oscillation condition
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           //make sure that we send the velocity command to the base
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           //check if we've tried to find a valid control for longer than our time limit
00926           if(ros::Time::now() > attempt_end){
00927             //we'll move into our obstacle clearing mode
00928             publishZeroVelocity();
00929             state_ = CLEARING;
00930             recovery_trigger_ = CONTROLLING_R;
00931           }
00932           else{
00933             //otherwise, if we can't find a valid control, we'll go back to planning
00934             last_valid_plan_ = ros::Time::now();
00935             state_ = PLANNING;
00936             publishZeroVelocity();
00937 
00938             //enable the planner thread in case it isn't running on a clock
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       //we'll try to clear out space with any user-provided recovery behaviors
00949       case CLEARING:
00950         ROS_DEBUG_NAMED("move_base","In clearing/recovery state");
00951         //we'll invoke whatever recovery behavior we're currently on if they're enabled
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           //we at least want to give the robot some time to stop oscillating after executing the behavior
00957           last_oscillation_reset_ = ros::Time::now();
00958 
00959           //we'll check if the recovery behavior actually worked
00960           ROS_DEBUG_NAMED("move_base_recovery","Going back to planning state");
00961           state_ = PLANNING;
00962 
00963           //update the index of the next recovery behavior that we'll try
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           //disable the planner thread
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         //disable the planner thread
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     //we aren't done yet
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               //check for recovery behaviors with the same name
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         //if we've made it to this point, we know that the list is legal so we'll create all the recovery behaviors
01041         for(int i = 0; i < behavior_list.size(); ++i){
01042           try{
01043             //check if a non fully qualified name has potentially been passed in
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                   //if we've found a match... we'll get the fully qualified name and break out of the loop
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             //shouldn't be possible, but it won't hurt to check
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             //initialize the recovery behavior with its name
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       //if no recovery_behaviors are specified, we'll just load the defaults
01083       return false;
01084     }
01085 
01086     //if we've made it here... we've constructed a recovery behavior list successfully
01087     return true;
01088   }
01089 
01090   //we'll load our default recovery behaviors here
01091   void MoveBase::loadDefaultRecoveryBehaviors(){
01092     recovery_behaviors_.clear();
01093     try{
01094       //we need to set some parameters based on what's been passed in to us to maintain backwards compatibility
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       //first, we'll load a recovery behavior to clear the costmap
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       //next, we'll load a recovery behavior to rotate in place
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       //next, we'll load a recovery behavior that will do an aggressive reset of the costmap
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       //we'll rotate in-place one more time
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     //if we shutdown our costmaps when we're deactivated... we'll do that now
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   //ros::MultiThreadedSpinner s;
01150   ros::spin();
01151 
01152   return(0);
01153 
01154 }


move_base
Author(s): Eitan Marder-Eppstein
autogenerated on Sat Dec 28 2013 17:14:19