planner_core.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2008, 2013, 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 Willow Garage, Inc. 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  *         David V. Lu!!
00037  *********************************************************************/
00038 #include <global_planner/planner_core.h>
00039 #include <pluginlib/class_list_macros.h>
00040 #include <tf/transform_listener.h>
00041 #include <costmap_2d/cost_values.h>
00042 #include <costmap_2d/costmap_2d.h>
00043 
00044 #include <global_planner/dijkstra.h>
00045 #include <global_planner/astar.h>
00046 #include <global_planner/grid_path.h>
00047 #include <global_planner/gradient_path.h>
00048 #include <global_planner/quadratic_calculator.h>
00049 
00050 //register this planner as a BaseGlobalPlanner plugin
00051 PLUGINLIB_EXPORT_CLASS(global_planner::GlobalPlanner, nav_core::BaseGlobalPlanner)
00052 
00053 namespace global_planner {
00054 
00055 void GlobalPlanner::outlineMap(unsigned char* costarr, int nx, int ny, unsigned char value) {
00056     unsigned char* pc = costarr;
00057     for (int i = 0; i < nx; i++)
00058         *pc++ = value;
00059     pc = costarr + (ny - 1) * nx;
00060     for (int i = 0; i < nx; i++)
00061         *pc++ = value;
00062     pc = costarr;
00063     for (int i = 0; i < ny; i++, pc += nx)
00064         *pc = value;
00065     pc = costarr + nx - 1;
00066     for (int i = 0; i < ny; i++, pc += nx)
00067         *pc = value;
00068 }
00069 
00070 GlobalPlanner::GlobalPlanner() :
00071         costmap_(NULL), initialized_(false), allow_unknown_(true) {
00072 }
00073 
00074 GlobalPlanner::GlobalPlanner(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id) :
00075         costmap_(NULL), initialized_(false), allow_unknown_(true) {
00076     //initialize the planner
00077     initialize(name, costmap, frame_id);
00078 }
00079 
00080 GlobalPlanner::~GlobalPlanner() {
00081     if (p_calc_)
00082         delete p_calc_;
00083     if (planner_)
00084         delete planner_;
00085     if (path_maker_)
00086         delete path_maker_;
00087     if (dsrv_)
00088         delete dsrv_;
00089 }
00090 
00091 void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) {
00092     initialize(name, costmap_ros->getCostmap(), costmap_ros->getGlobalFrameID());
00093 }
00094 
00095 void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id) {
00096     if (!initialized_) {
00097         ros::NodeHandle private_nh("~/" + name);
00098         costmap_ = costmap;
00099         frame_id_ = frame_id;
00100 
00101         unsigned int cx = costmap->getSizeInCellsX(), cy = costmap->getSizeInCellsY();
00102 
00103         private_nh.param("old_navfn_behavior", old_navfn_behavior_, false);
00104         if(!old_navfn_behavior_)
00105             convert_offset_ = 0.5;
00106         else
00107             convert_offset_ = 0.0;
00108 
00109         bool use_quadratic;
00110         private_nh.param("use_quadratic", use_quadratic, true);
00111         if (use_quadratic)
00112             p_calc_ = new QuadraticCalculator(cx, cy);
00113         else
00114             p_calc_ = new PotentialCalculator(cx, cy);
00115 
00116         bool use_dijkstra;
00117         private_nh.param("use_dijkstra", use_dijkstra, true);
00118         if (use_dijkstra)
00119         {
00120             DijkstraExpansion* de = new DijkstraExpansion(p_calc_, cx, cy);
00121             if(!old_navfn_behavior_)
00122                 de->setPreciseStart(true);
00123             planner_ = de;
00124         }
00125         else
00126             planner_ = new AStarExpansion(p_calc_, cx, cy);
00127 
00128         bool use_grid_path;
00129         private_nh.param("use_grid_path", use_grid_path, false);
00130         if (use_grid_path)
00131             path_maker_ = new GridPath(p_calc_);
00132         else
00133             path_maker_ = new GradientPath(p_calc_);
00134             
00135         orientation_filter_ = new OrientationFilter();
00136 
00137         plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
00138         potential_pub_ = private_nh.advertise<nav_msgs::OccupancyGrid>("potential", 1);
00139 
00140         private_nh.param("allow_unknown", allow_unknown_, true);
00141         planner_->setHasUnknown(allow_unknown_);
00142         private_nh.param("planner_window_x", planner_window_x_, 0.0);
00143         private_nh.param("planner_window_y", planner_window_y_, 0.0);
00144         private_nh.param("default_tolerance", default_tolerance_, 0.0);
00145         private_nh.param("publish_scale", publish_scale_, 100);
00146 
00147         double costmap_pub_freq;
00148         private_nh.param("planner_costmap_publish_frequency", costmap_pub_freq, 0.0);
00149 
00150         //get the tf prefix
00151         ros::NodeHandle prefix_nh;
00152         tf_prefix_ = tf::getPrefixParam(prefix_nh);
00153 
00154         make_plan_srv_ = private_nh.advertiseService("make_plan", &GlobalPlanner::makePlanService, this);
00155 
00156         dsrv_ = new dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>(ros::NodeHandle("~/" + name));
00157         dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>::CallbackType cb = boost::bind(
00158                 &GlobalPlanner::reconfigureCB, this, _1, _2);
00159         dsrv_->setCallback(cb);
00160 
00161         initialized_ = true;
00162     } else
00163         ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing");
00164 
00165 }
00166 
00167 void GlobalPlanner::reconfigureCB(global_planner::GlobalPlannerConfig& config, uint32_t level) {
00168     planner_->setLethalCost(config.lethal_cost);
00169     path_maker_->setLethalCost(config.lethal_cost);
00170     planner_->setNeutralCost(config.neutral_cost);
00171     planner_->setFactor(config.cost_factor);
00172     publish_potential_ = config.publish_potential;
00173     orientation_filter_->setMode(config.orientation_mode);
00174 }
00175 
00176 void GlobalPlanner::clearRobotCell(const tf::Stamped<tf::Pose>& global_pose, unsigned int mx, unsigned int my) {
00177     if (!initialized_) {
00178         ROS_ERROR(
00179                 "This planner has not been initialized yet, but it is being used, please call initialize() before use");
00180         return;
00181     }
00182 
00183     //set the associated costs in the cost map to be free
00184     costmap_->setCost(mx, my, costmap_2d::FREE_SPACE);
00185 }
00186 
00187 bool GlobalPlanner::makePlanService(nav_msgs::GetPlan::Request& req, nav_msgs::GetPlan::Response& resp) {
00188     makePlan(req.start, req.goal, resp.plan.poses);
00189 
00190     resp.plan.header.stamp = ros::Time::now();
00191     resp.plan.header.frame_id = frame_id_;
00192 
00193     return true;
00194 }
00195 
00196 void GlobalPlanner::mapToWorld(double mx, double my, double& wx, double& wy) {
00197     wx = costmap_->getOriginX() + (mx+convert_offset_) * costmap_->getResolution();
00198     wy = costmap_->getOriginY() + (my+convert_offset_) * costmap_->getResolution();
00199 }
00200 
00201 bool GlobalPlanner::worldToMap(double wx, double wy, double& mx, double& my) {
00202     double origin_x = costmap_->getOriginX(), origin_y = costmap_->getOriginY();
00203     double resolution = costmap_->getResolution();
00204 
00205     if (wx < origin_x || wy < origin_y)
00206         return false;
00207 
00208     mx = (wx - origin_x) / resolution - convert_offset_;
00209     my = (wy - origin_y) / resolution - convert_offset_;
00210 
00211     if (mx < costmap_->getSizeInCellsX() && my < costmap_->getSizeInCellsY())
00212         return true;
00213 
00214     return false;
00215 }
00216 
00217 bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
00218                            std::vector<geometry_msgs::PoseStamped>& plan) {
00219     return makePlan(start, goal, default_tolerance_, plan);
00220 }
00221 
00222 bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
00223                            double tolerance, std::vector<geometry_msgs::PoseStamped>& plan) {
00224     boost::mutex::scoped_lock lock(mutex_);
00225     if (!initialized_) {
00226         ROS_ERROR(
00227                 "This planner has not been initialized yet, but it is being used, please call initialize() before use");
00228         return false;
00229     }
00230 
00231     //clear the plan, just in case
00232     plan.clear();
00233 
00234     ros::NodeHandle n;
00235     std::string global_frame = frame_id_;
00236 
00237     //until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
00238     if (tf::resolve(tf_prefix_, goal.header.frame_id) != tf::resolve(tf_prefix_, global_frame)) {
00239         ROS_ERROR(
00240                 "The goal pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", tf::resolve(tf_prefix_, global_frame).c_str(), tf::resolve(tf_prefix_, goal.header.frame_id).c_str());
00241         return false;
00242     }
00243 
00244     if (tf::resolve(tf_prefix_, start.header.frame_id) != tf::resolve(tf_prefix_, global_frame)) {
00245         ROS_ERROR(
00246                 "The start pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", tf::resolve(tf_prefix_, global_frame).c_str(), tf::resolve(tf_prefix_, start.header.frame_id).c_str());
00247         return false;
00248     }
00249 
00250     double wx = start.pose.position.x;
00251     double wy = start.pose.position.y;
00252 
00253     unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i;
00254     double start_x, start_y, goal_x, goal_y;
00255 
00256     if (!costmap_->worldToMap(wx, wy, start_x_i, start_y_i)) {
00257         ROS_WARN(
00258                 "The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?");
00259         return false;
00260     }
00261     if(old_navfn_behavior_){
00262         start_x = start_x_i;
00263         start_y = start_y_i;
00264     }else{
00265         worldToMap(wx, wy, start_x, start_y);
00266     }
00267 
00268     wx = goal.pose.position.x;
00269     wy = goal.pose.position.y;
00270 
00271     if (!costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i)) {
00272         ROS_WARN_THROTTLE(1.0,
00273                 "The goal sent to the global planner is off the global costmap. Planning will always fail to this goal.");
00274         return false;
00275     }
00276     if(old_navfn_behavior_){
00277         goal_x = goal_x_i;
00278         goal_y = goal_y_i;
00279     }else{
00280         worldToMap(wx, wy, goal_x, goal_y);
00281     }
00282 
00283     //clear the starting cell within the costmap because we know it can't be an obstacle
00284     tf::Stamped<tf::Pose> start_pose;
00285     tf::poseStampedMsgToTF(start, start_pose);
00286     clearRobotCell(start_pose, start_x_i, start_y_i);
00287 
00288     int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();
00289 
00290     //make sure to resize the underlying array that Navfn uses
00291     p_calc_->setSize(nx, ny);
00292     planner_->setSize(nx, ny);
00293     path_maker_->setSize(nx, ny);
00294     potential_array_ = new float[nx * ny];
00295 
00296     outlineMap(costmap_->getCharMap(), nx, ny, costmap_2d::LETHAL_OBSTACLE);
00297 
00298     bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y,
00299                                                     nx * ny * 2, potential_array_);
00300 
00301     if(!old_navfn_behavior_)
00302         planner_->clearEndpoint(costmap_->getCharMap(), potential_array_, goal_x_i, goal_y_i, 2);
00303     if(publish_potential_)
00304         publishPotential(potential_array_);
00305 
00306     if (found_legal) {
00307         //extract the plan
00308         if (getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan)) {
00309             //make sure the goal we push on has the same timestamp as the rest of the plan
00310             geometry_msgs::PoseStamped goal_copy = goal;
00311             goal_copy.header.stamp = ros::Time::now();
00312             plan.push_back(goal_copy);
00313         } else {
00314             ROS_ERROR("Failed to get a plan from potential when a legal potential was found. This shouldn't happen.");
00315         }
00316     }else{
00317         ROS_ERROR("Failed to get a plan.");
00318     }
00319 
00320     // add orientations if needed
00321     orientation_filter_->processPath(start, plan);
00322     
00323     //publish the plan for visualization purposes
00324     publishPlan(plan);
00325     delete potential_array_;
00326     return !plan.empty();
00327 }
00328 
00329 void GlobalPlanner::publishPlan(const std::vector<geometry_msgs::PoseStamped>& path) {
00330     if (!initialized_) {
00331         ROS_ERROR(
00332                 "This planner has not been initialized yet, but it is being used, please call initialize() before use");
00333         return;
00334     }
00335 
00336     //create a message for the plan
00337     nav_msgs::Path gui_path;
00338     gui_path.poses.resize(path.size());
00339 
00340     if (!path.empty()) {
00341         gui_path.header.frame_id = path[0].header.frame_id;
00342         gui_path.header.stamp = path[0].header.stamp;
00343     }
00344 
00345     // Extract the plan in world co-ordinates, we assume the path is all in the same frame
00346     for (unsigned int i = 0; i < path.size(); i++) {
00347         gui_path.poses[i] = path[i];
00348     }
00349 
00350     plan_pub_.publish(gui_path);
00351 }
00352 
00353 bool GlobalPlanner::getPlanFromPotential(double start_x, double start_y, double goal_x, double goal_y,
00354                                       const geometry_msgs::PoseStamped& goal,
00355                                        std::vector<geometry_msgs::PoseStamped>& plan) {
00356     if (!initialized_) {
00357         ROS_ERROR(
00358                 "This planner has not been initialized yet, but it is being used, please call initialize() before use");
00359         return false;
00360     }
00361 
00362     std::string global_frame = frame_id_;
00363 
00364     //clear the plan, just in case
00365     plan.clear();
00366 
00367     std::vector<std::pair<float, float> > path;
00368 
00369     if (!path_maker_->getPath(potential_array_, start_x, start_y, goal_x, goal_y, path)) {
00370         ROS_ERROR("NO PATH!");
00371         return false;
00372     }
00373 
00374     ros::Time plan_time = ros::Time::now();
00375     for (int i = path.size() -1; i>=0; i--) {
00376         std::pair<float, float> point = path[i];
00377         //convert the plan to world coordinates
00378         double world_x, world_y;
00379         mapToWorld(point.first, point.second, world_x, world_y);
00380 
00381         geometry_msgs::PoseStamped pose;
00382         pose.header.stamp = plan_time;
00383         pose.header.frame_id = global_frame;
00384         pose.pose.position.x = world_x;
00385         pose.pose.position.y = world_y;
00386         pose.pose.position.z = 0.0;
00387         pose.pose.orientation.x = 0.0;
00388         pose.pose.orientation.y = 0.0;
00389         pose.pose.orientation.z = 0.0;
00390         pose.pose.orientation.w = 1.0;
00391         plan.push_back(pose);
00392     }
00393     if(old_navfn_behavior_){
00394             plan.push_back(goal);
00395     }
00396     return !plan.empty();
00397 }
00398 
00399 void GlobalPlanner::publishPotential(float* potential)
00400 {
00401     int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();
00402     double resolution = costmap_->getResolution();
00403     nav_msgs::OccupancyGrid grid;
00404     // Publish Whole Grid
00405     grid.header.frame_id = frame_id_;
00406     grid.header.stamp = ros::Time::now();
00407     grid.info.resolution = resolution;
00408 
00409     grid.info.width = nx;
00410     grid.info.height = ny;
00411 
00412     double wx, wy;
00413     costmap_->mapToWorld(0, 0, wx, wy);
00414     grid.info.origin.position.x = wx - resolution / 2;
00415     grid.info.origin.position.y = wy - resolution / 2;
00416     grid.info.origin.position.z = 0.0;
00417     grid.info.origin.orientation.w = 1.0;
00418 
00419     grid.data.resize(nx * ny);
00420 
00421     float max = 0.0;
00422     for (unsigned int i = 0; i < grid.data.size(); i++) {
00423         float potential = potential_array_[i];
00424         if (potential < POT_HIGH) {
00425             if (potential > max) {
00426                 max = potential;
00427             }
00428         }
00429     }
00430 
00431     for (unsigned int i = 0; i < grid.data.size(); i++) {
00432         if (potential_array_[i] >= POT_HIGH) {
00433             grid.data[i] = -1;
00434         } else
00435             grid.data[i] = potential_array_[i] * publish_scale_ / max;
00436     }
00437     potential_pub_.publish(grid);
00438 }
00439 
00440 } //end namespace global_planner
00441 


global_planner
Author(s): David Lu!!
autogenerated on Sun Mar 3 2019 03:46:53