navfn_ros.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 *********************************************************************/
00037 #include <navfn/navfn_ros.h>
00038 #include <pluginlib/class_list_macros.h>
00039 #include <tf/transform_listener.h>
00040 #include <costmap_2d/cost_values.h>
00041 #include <costmap_2d/costmap_2d.h>
00042 
00043 #include <pcl_conversions/pcl_conversions.h>
00044 
00045 //register this planner as a BaseGlobalPlanner plugin
00046 PLUGINLIB_DECLARE_CLASS(navfn, NavfnROS, navfn::NavfnROS, nav_core::BaseGlobalPlanner)
00047 
00048 namespace navfn {
00049 
00050   NavfnROS::NavfnROS() 
00051     : costmap_ros_(NULL),  planner_(), initialized_(false), allow_unknown_(true) {}
00052 
00053   NavfnROS::NavfnROS(std::string name, costmap_2d::Costmap2DROS* costmap_ros) 
00054     : costmap_ros_(NULL),  planner_(), initialized_(false), allow_unknown_(true) {
00055       //initialize the planner
00056       initialize(name, costmap_ros);
00057   }
00058 
00059   void NavfnROS::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
00060     if(!initialized_){
00061       costmap_ros_ = costmap_ros;
00062       costmap_2d::Costmap2D* costmap = costmap_ros_->getCostmap();
00063       planner_ = boost::shared_ptr<NavFn>(new NavFn(costmap->getSizeInCellsX(), costmap->getSizeInCellsY()));
00064 
00065       ros::NodeHandle private_nh("~/" + name);
00066 
00067       plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
00068 
00069       private_nh.param("visualize_potential", visualize_potential_, false);
00070 
00071       //if we're going to visualize the potential array we need to advertise
00072       if(visualize_potential_)
00073         potarr_pub_.advertise(private_nh, "potential", 1);
00074 
00075       private_nh.param("allow_unknown", allow_unknown_, true);
00076       private_nh.param("planner_window_x", planner_window_x_, 0.0);
00077       private_nh.param("planner_window_y", planner_window_y_, 0.0);
00078       private_nh.param("default_tolerance", default_tolerance_, 0.0);
00079 
00080       //get the tf prefix
00081       ros::NodeHandle prefix_nh;
00082       tf_prefix_ = tf::getPrefixParam(prefix_nh);
00083 
00084       make_plan_srv_ =  private_nh.advertiseService("make_plan", &NavfnROS::makePlanService, this);
00085 
00086       initialized_ = true;
00087     }
00088     else
00089       ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing");
00090   }
00091 
00092   bool NavfnROS::validPointPotential(const geometry_msgs::Point& world_point){
00093     return validPointPotential(world_point, default_tolerance_);
00094   }
00095 
00096   bool NavfnROS::validPointPotential(const geometry_msgs::Point& world_point, double tolerance){
00097     if(!initialized_){
00098       ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
00099       return false;
00100     }
00101 
00102     double resolution = costmap_ros_->getCostmap()->getResolution();
00103     geometry_msgs::Point p;
00104     p = world_point;
00105 
00106     p.y = world_point.y - tolerance;
00107 
00108     while(p.y <= world_point.y + tolerance){
00109       p.x = world_point.x - tolerance;
00110       while(p.x <= world_point.x + tolerance){
00111         double potential = getPointPotential(p);
00112         if(potential < POT_HIGH){
00113           return true;
00114         }
00115         p.x += resolution;
00116       }
00117       p.y += resolution;
00118     }
00119 
00120     return false;
00121   }
00122 
00123   double NavfnROS::getPointPotential(const geometry_msgs::Point& world_point){
00124     if(!initialized_){
00125       ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
00126       return -1.0;
00127     }
00128 
00129     unsigned int mx, my;
00130     if(!costmap_ros_->getCostmap()->worldToMap(world_point.x, world_point.y, mx, my))
00131       return DBL_MAX;
00132 
00133     unsigned int index = my * planner_->nx + mx;
00134     return planner_->potarr[index];
00135   }
00136 
00137   bool NavfnROS::computePotential(const geometry_msgs::Point& world_point){
00138     if(!initialized_){
00139       ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
00140       return false;
00141     }
00142     
00143     costmap_2d::Costmap2D* costmap = costmap_ros_->getCostmap();
00144 
00145     //make sure to resize the underlying array that Navfn uses
00146     planner_->setNavArr(costmap->getSizeInCellsX(), costmap->getSizeInCellsY());
00147     planner_->setCostmap(costmap->getCharMap(), true, allow_unknown_);
00148 
00149     unsigned int mx, my;
00150     if(!costmap->worldToMap(world_point.x, world_point.y, mx, my))
00151       return false;
00152 
00153     int map_start[2];
00154     map_start[0] = 0;
00155     map_start[1] = 0;
00156 
00157     int map_goal[2];
00158     map_goal[0] = mx;
00159     map_goal[1] = my;
00160 
00161     planner_->setStart(map_start);
00162     planner_->setGoal(map_goal);
00163 
00164     return planner_->calcNavFnDijkstra();
00165   }
00166 
00167   void NavfnROS::clearRobotCell(const tf::Stamped<tf::Pose>& global_pose, unsigned int mx, unsigned int my){
00168     if(!initialized_){
00169       ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
00170       return;
00171     }
00172 
00173     //set the associated costs in the cost map to be free
00174     costmap_ros_->getCostmap()->setCost(mx, my, costmap_2d::FREE_SPACE);
00175   }
00176 
00177   bool NavfnROS::makePlanService(nav_msgs::GetPlan::Request& req, nav_msgs::GetPlan::Response& resp){
00178     makePlan(req.start, req.goal, resp.plan.poses);
00179 
00180     resp.plan.header.stamp = ros::Time::now();
00181     resp.plan.header.frame_id = costmap_ros_->getGlobalFrameID();
00182 
00183     return true;
00184   } 
00185 
00186   void NavfnROS::mapToWorld(double mx, double my, double& wx, double& wy) {
00187     costmap_2d::Costmap2D* costmap = costmap_ros_->getCostmap();
00188     wx = costmap->getOriginX() + mx * costmap->getResolution();
00189     wy = costmap->getOriginY() + my * costmap->getResolution();
00190   }
00191 
00192   bool NavfnROS::makePlan(const geometry_msgs::PoseStamped& start, 
00193       const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan){
00194     return makePlan(start, goal, default_tolerance_, plan);
00195   }
00196 
00197   bool NavfnROS::makePlan(const geometry_msgs::PoseStamped& start, 
00198       const geometry_msgs::PoseStamped& goal, double tolerance, std::vector<geometry_msgs::PoseStamped>& plan){
00199     boost::mutex::scoped_lock lock(mutex_);
00200     if(!initialized_){
00201       ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
00202       return false;
00203     }
00204 
00205     //clear the plan, just in case
00206     plan.clear();
00207 
00208     ros::NodeHandle n;
00209     costmap_2d::Costmap2D* costmap = costmap_ros_->getCostmap();
00210     std::string global_frame = costmap_ros_->getGlobalFrameID();
00211 
00212     //until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
00213     if(tf::resolve(tf_prefix_, goal.header.frame_id) != tf::resolve(tf_prefix_, global_frame)){
00214       ROS_ERROR("The goal pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", 
00215                 tf::resolve(tf_prefix_, global_frame).c_str(), tf::resolve(tf_prefix_, goal.header.frame_id).c_str());
00216       return false;
00217     }
00218 
00219     if(tf::resolve(tf_prefix_, start.header.frame_id) != tf::resolve(tf_prefix_, global_frame)){
00220       ROS_ERROR("The start pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", 
00221                 tf::resolve(tf_prefix_, global_frame).c_str(), tf::resolve(tf_prefix_, start.header.frame_id).c_str());
00222       return false;
00223     }
00224 
00225     double wx = start.pose.position.x;
00226     double wy = start.pose.position.y;
00227 
00228     unsigned int mx, my;
00229     if(!costmap->worldToMap(wx, wy, mx, my)){
00230       ROS_WARN("The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?");
00231       return false;
00232     }
00233 
00234     //clear the starting cell within the costmap because we know it can't be an obstacle
00235     tf::Stamped<tf::Pose> start_pose;
00236     tf::poseStampedMsgToTF(start, start_pose);
00237     clearRobotCell(start_pose, mx, my);
00238 
00239 #if 0
00240     {
00241       static int n = 0;
00242       static char filename[1000];
00243       snprintf( filename, 1000, "navfnros-makeplan-costmapB-%04d.pgm", n++ );
00244       costmap->saveRawMap( std::string( filename ));
00245     }
00246 #endif
00247 
00248     //make sure to resize the underlying array that Navfn uses
00249     planner_->setNavArr(costmap->getSizeInCellsX(), costmap->getSizeInCellsY());
00250     planner_->setCostmap(costmap->getCharMap(), true, allow_unknown_);
00251 
00252 #if 0
00253     {
00254       static int n = 0;
00255       static char filename[1000];
00256       snprintf( filename, 1000, "navfnros-makeplan-costmapC-%04d", n++ );
00257       planner_->savemap( filename );
00258     }
00259 #endif
00260 
00261     int map_start[2];
00262     map_start[0] = mx;
00263     map_start[1] = my;
00264 
00265     wx = goal.pose.position.x;
00266     wy = goal.pose.position.y;
00267 
00268     if(!costmap->worldToMap(wx, wy, mx, my)){
00269       if(tolerance <= 0.0){
00270         ROS_WARN_THROTTLE(1.0, "The goal sent to the navfn planner is off the global costmap. Planning will always fail to this goal.");
00271         return false;
00272       }
00273       mx = 0;
00274       my = 0;
00275     }
00276 
00277     int map_goal[2];
00278     map_goal[0] = mx;
00279     map_goal[1] = my;
00280 
00281     planner_->setStart(map_goal);
00282     planner_->setGoal(map_start);
00283 
00284     //bool success = planner_->calcNavFnAstar();
00285     planner_->calcNavFnDijkstra(true);
00286 
00287     double resolution = costmap->getResolution();
00288     geometry_msgs::PoseStamped p, best_pose;
00289     p = goal;
00290 
00291     bool found_legal = false;
00292     double best_sdist = DBL_MAX;
00293 
00294     p.pose.position.y = goal.pose.position.y - tolerance;
00295 
00296     while(p.pose.position.y <= goal.pose.position.y + tolerance){
00297       p.pose.position.x = goal.pose.position.x - tolerance;
00298       while(p.pose.position.x <= goal.pose.position.x + tolerance){
00299         double potential = getPointPotential(p.pose.position);
00300         double sdist = sq_distance(p, goal);
00301         if(potential < POT_HIGH && sdist < best_sdist){
00302           best_sdist = sdist;
00303           best_pose = p;
00304           found_legal = true;
00305         }
00306         p.pose.position.x += resolution;
00307       }
00308       p.pose.position.y += resolution;
00309     }
00310 
00311     if(found_legal){
00312       //extract the plan
00313       if(getPlanFromPotential(best_pose, plan)){
00314         //make sure the goal we push on has the same timestamp as the rest of the plan
00315         geometry_msgs::PoseStamped goal_copy = best_pose;
00316         goal_copy.header.stamp = ros::Time::now();
00317         plan.push_back(goal_copy);
00318       }
00319       else{
00320         ROS_ERROR("Failed to get a plan from potential when a legal potential was found. This shouldn't happen.");
00321       }
00322     }
00323 
00324     if (visualize_potential_){
00325       //publish potential array
00326       pcl::PointCloud<PotarrPoint> pot_area;
00327       pot_area.header.frame_id = global_frame;
00328       pot_area.points.clear();
00329       std_msgs::Header header;
00330       pcl_conversions::fromPCL(pot_area.header, header);
00331       header.stamp = ros::Time::now();
00332       pot_area.header = pcl_conversions::toPCL(header);
00333 
00334       PotarrPoint pt;
00335       float *pp = planner_->potarr;
00336       double pot_x, pot_y;
00337       for (unsigned int i = 0; i < (unsigned int)planner_->ny*planner_->nx ; i++)
00338       {
00339         if (pp[i] < 10e7)
00340         {
00341           mapToWorld(i%planner_->nx, i/planner_->nx, pot_x, pot_y);
00342           pt.x = pot_x;
00343           pt.y = pot_y;
00344           pt.z = pp[i]/pp[planner_->start[1]*planner_->nx + planner_->start[0]]*20;
00345           pt.pot_value = pp[i];
00346           pot_area.push_back(pt);
00347         }
00348       }
00349       potarr_pub_.publish(pot_area);
00350     }
00351 
00352     //publish the plan for visualization purposes
00353     publishPlan(plan, 0.0, 1.0, 0.0, 0.0);
00354 
00355     return !plan.empty();
00356   }
00357 
00358   void NavfnROS::publishPlan(const std::vector<geometry_msgs::PoseStamped>& path, double r, double g, double b, double a){
00359     if(!initialized_){
00360       ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
00361       return;
00362     }
00363 
00364     //create a message for the plan 
00365     nav_msgs::Path gui_path;
00366     gui_path.poses.resize(path.size());
00367 
00368     if(!path.empty())
00369     {
00370       gui_path.header.frame_id = path[0].header.frame_id;
00371       gui_path.header.stamp = path[0].header.stamp;
00372     }
00373 
00374     // Extract the plan in world co-ordinates, we assume the path is all in the same frame
00375     for(unsigned int i=0; i < path.size(); i++){
00376       gui_path.poses[i] = path[i];
00377     }
00378 
00379     plan_pub_.publish(gui_path);
00380   }
00381 
00382   bool NavfnROS::getPlanFromPotential(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan){
00383     if(!initialized_){
00384       ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
00385       return false;
00386     }
00387     
00388     costmap_2d::Costmap2D* costmap = costmap_ros_->getCostmap();
00389     std::string global_frame = costmap_ros_->getGlobalFrameID();
00390 
00391     //clear the plan, just in case
00392     plan.clear();
00393 
00394     //until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
00395     if(tf::resolve(tf_prefix_, goal.header.frame_id) != tf::resolve(tf_prefix_, global_frame)){
00396       ROS_ERROR("The goal pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", 
00397                 tf::resolve(tf_prefix_, global_frame).c_str(), tf::resolve(tf_prefix_, goal.header.frame_id).c_str());
00398       return false;
00399     }
00400 
00401     double wx = goal.pose.position.x;
00402     double wy = goal.pose.position.y;
00403 
00404     //the potential has already been computed, so we won't update our copy of the costmap
00405     unsigned int mx, my;
00406     if(!costmap->worldToMap(wx, wy, mx, my)){
00407       ROS_WARN_THROTTLE(1.0, "The goal sent to the navfn planner is off the global costmap. Planning will always fail to this goal.");
00408       return false;
00409     }
00410 
00411     int map_goal[2];
00412     map_goal[0] = mx;
00413     map_goal[1] = my;
00414 
00415     planner_->setStart(map_goal);
00416 
00417     planner_->calcPath(costmap->getSizeInCellsX() * 4);
00418 
00419     //extract the plan
00420     float *x = planner_->getPathX();
00421     float *y = planner_->getPathY();
00422     int len = planner_->getPathLen();
00423     ros::Time plan_time = ros::Time::now();
00424 
00425     for(int i = len - 1; i >= 0; --i){
00426       //convert the plan to world coordinates
00427       double world_x, world_y;
00428       mapToWorld(x[i], y[i], world_x, world_y);
00429 
00430       geometry_msgs::PoseStamped pose;
00431       pose.header.stamp = plan_time;
00432       pose.header.frame_id = global_frame;
00433       pose.pose.position.x = world_x;
00434       pose.pose.position.y = world_y;
00435       pose.pose.position.z = 0.0;
00436       pose.pose.orientation.x = 0.0;
00437       pose.pose.orientation.y = 0.0;
00438       pose.pose.orientation.z = 0.0;
00439       pose.pose.orientation.w = 1.0;
00440       plan.push_back(pose);
00441     }
00442 
00443     //publish the plan for visualization purposes
00444     publishPlan(plan, 0.0, 1.0, 0.0, 0.0);
00445     return !plan.empty();
00446   }
00447 };


navfn
Author(s): Kurt Konolige, Eitan Marder-Eppstein, contradict@gmail.com
autogenerated on Wed Aug 2 2017 03:13:07