sbpl_lattice_planner: sbpl_lattice_planner.cpp Source 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: Mike Phillips
00036 *********************************************************************/
00037 
00038 #include <sbpl_lattice_planner/sbpl_lattice_planner.h>
00039 #include <pluginlib/class_list_macros.h>
00040 #include <nav_msgs/Path.h>
00041 #include <sbpl_lattice_planner/SBPLLatticePlannerStats.h>
00042 
00043 using namespace std;
00044 using namespace ros;
00045 
00046 
00047 PLUGINLIB_REGISTER_CLASS(SBPLLatticePlanner, sbpl_lattice_planner::SBPLLatticePlanner, nav_core::BaseGlobalPlanner);
00048 
00049 namespace sbpl_lattice_planner{
00050 
00051 class LatticeSCQ : public StateChangeQuery{
00052   public:
00053     LatticeSCQ(EnvironmentNAVXYTHETALAT* env, std::vector<nav2dcell_t> const & changedcellsV)
00054       : env_(env), changedcellsV_(changedcellsV) {
00055     }
00056 
00057     // lazy init, because we do not always end up calling this method
00058     virtual std::vector<int> const * getPredecessors() const{
00059       if(predsOfChangedCells_.empty() && !changedcellsV_.empty())
00060         env_->GetPredsofChangedEdges(&changedcellsV_, &predsOfChangedCells_);
00061       return &predsOfChangedCells_;
00062     }
00063 
00064     // lazy init, because we do not always end up calling this method
00065     virtual std::vector<int> const * getSuccessors() const{
00066       if(succsOfChangedCells_.empty() && !changedcellsV_.empty())
00067         env_->GetSuccsofChangedEdges(&changedcellsV_, &succsOfChangedCells_);
00068       return &succsOfChangedCells_;
00069     }
00070 
00071     EnvironmentNAVXYTHETALAT * env_;
00072     std::vector<nav2dcell_t> const & changedcellsV_;
00073     mutable std::vector<int> predsOfChangedCells_;
00074     mutable std::vector<int> succsOfChangedCells_;
00075 };
00076 
00077 SBPLLatticePlanner::SBPLLatticePlanner()
00078   : initialized_(false), costmap_ros_(NULL){
00079 }
00080 
00081 SBPLLatticePlanner::SBPLLatticePlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros) 
00082   : initialized_(false), costmap_ros_(NULL){
00083   initialize(name, costmap_ros);
00084 }
00085 
00086     
00087 void SBPLLatticePlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
00088   if(!initialized_){
00089     ros::NodeHandle private_nh("~/"+name);
00090     ros::NodeHandle nh(name);
00091     
00092     ROS_INFO("Name is %s", name.c_str());
00093 
00094     private_nh.param("planner_type", planner_type_, string("ARAPlanner"));
00095     private_nh.param("allocated_time", allocated_time_, 10.0);
00096     private_nh.param("initial_epsilon",initial_epsilon_,3.0);
00097     private_nh.param("environment_type", environment_type_, string("XYThetaLattice"));
00098     private_nh.param("forward_search", forward_search_, bool(false));
00099     private_nh.param("primitive_filename",primitive_filename_,string(""));
00100     private_nh.param("force_scratch_limit",force_scratch_limit_,500);
00101 
00102     double nominalvel_mpersecs, timetoturn45degsinplace_secs;
00103     private_nh.param("nominalvel_mpersecs", nominalvel_mpersecs, 0.4);
00104     private_nh.param("timetoturn45degsinplace_secs", timetoturn45degsinplace_secs, 0.6);
00105 
00106     int lethal_obstacle;
00107     private_nh.param("lethal_obstacle",lethal_obstacle,20);
00108     lethal_obstacle_ = (unsigned char) lethal_obstacle;
00109     inscribed_inflated_obstacle_ = lethal_obstacle_-1;
00110     sbpl_cost_multiplier_ = (unsigned char) (costmap_2d::INSCRIBED_INFLATED_OBSTACLE/inscribed_inflated_obstacle_ + 1);
00111     ROS_DEBUG("SBPL: lethal: %uz, inscribed inflated: %uz, multiplier: %uz",lethal_obstacle,inscribed_inflated_obstacle_,sbpl_cost_multiplier_);
00112     
00113     costmap_ros_ = costmap_ros;
00114     costmap_ros_->clearRobotFootprint();
00115     costmap_ros_->getCostmapCopy(cost_map_);
00116 
00117     std::vector<geometry_msgs::Point> footprint = costmap_ros_->getRobotFootprint();
00118 
00119     if ("XYThetaLattice" == environment_type_){
00120       ROS_DEBUG("Using a 3D costmap for theta lattice\n");
00121       env_ = new EnvironmentNAVXYTHETALAT();
00122     }
00123     else{
00124       ROS_ERROR("XYThetaLattice is currently the only supported environment!\n");
00125       exit(1);
00126     }
00127 
00128     if(!env_->SetEnvParameter("cost_inscribed_thresh",costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE))){
00129       ROS_ERROR("Failed to set cost_inscribed_thresh parameter");
00130       exit(1);
00131     }
00132     if(!env_->SetEnvParameter("cost_possibly_circumscribed_thresh", costMapCostToSBPLCost(cost_map_.getCircumscribedCost()))){
00133       ROS_ERROR("Failed to set cost_possibly_circumscribed_thresh parameter");
00134       exit(1);
00135     }
00136     int obst_cost_thresh = costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE);
00137     vector<sbpl_2Dpt_t> perimeterptsV;
00138     perimeterptsV.reserve(footprint.size());
00139     for (size_t ii(0); ii < footprint.size(); ++ii) {
00140       sbpl_2Dpt_t pt;
00141       pt.x = footprint[ii].x;
00142       pt.y = footprint[ii].y;
00143       perimeterptsV.push_back(pt);
00144     }
00145 
00146     bool ret;
00147     try{
00148       ret = env_->InitializeEnv(costmap_ros_->getSizeInCellsX(), // width
00149                                 costmap_ros_->getSizeInCellsY(), // height
00150                                 0, // mapdata
00151                                 0, 0, 0, // start (x, y, theta, t)
00152                                 0, 0, 0, // goal (x, y, theta)
00153                                 0, 0, 0, //goal tolerance
00154                                 perimeterptsV, costmap_ros_->getResolution(), nominalvel_mpersecs,
00155                                 timetoturn45degsinplace_secs, obst_cost_thresh,
00156                                 primitive_filename_.c_str());
00157     }
00158     catch(SBPL_Exception e){
00159       ROS_ERROR("SBPL encountered a fatal exception!");
00160       ret = false;
00161     }
00162     if(!ret){
00163       ROS_ERROR("SBPL initialization failed!");
00164       exit(1);
00165     }
00166     for (ssize_t ix(0); ix < costmap_ros_->getSizeInCellsX(); ++ix)
00167       for (ssize_t iy(0); iy < costmap_ros_->getSizeInCellsY(); ++iy)
00168         env_->UpdateCost(ix, iy, costMapCostToSBPLCost(cost_map_.getCost(ix,iy)));
00169 
00170     if ("ARAPlanner" == planner_type_){
00171       ROS_INFO("Planning with ARA*");
00172       planner_ = new ARAPlanner(env_, forward_search_);
00173     }
00174     else if ("ADPlanner" == planner_type_){
00175       ROS_INFO("Planning with AD*");
00176       planner_ = new ADPlanner(env_, forward_search_);
00177     }
00178     else{
00179       ROS_ERROR("ARAPlanner and ADPlanner are currently the only supported planners!\n");
00180       exit(1);
00181     }
00182 
00183     ROS_INFO("[sbpl_lattice_planner] Initialized successfully");
00184     plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
00185     stats_publisher_ = private_nh.advertise<sbpl_lattice_planner::SBPLLatticePlannerStats>("sbpl_lattice_planner_stats", 1);
00186     
00187     initialized_ = true;
00188   }
00189 }
00190   
00191 //Taken from Sachin's sbpl_cart_planner
00192 //This rescales the costmap according to a rosparam which sets the obstacle cost
00193 unsigned char SBPLLatticePlanner::costMapCostToSBPLCost(unsigned char newcost){
00194   if(newcost == costmap_2d::LETHAL_OBSTACLE)
00195     return lethal_obstacle_;
00196   else if(newcost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
00197     return inscribed_inflated_obstacle_;
00198   else if(newcost == 0 || newcost == costmap_2d::NO_INFORMATION)
00199     return 0;
00200   else
00201     return (unsigned char) (newcost/sbpl_cost_multiplier_ + 0.5);
00202 }
00203 
00204 void SBPLLatticePlanner::publishStats(int solution_cost, int solution_size, 
00205                                       const geometry_msgs::PoseStamped& start, 
00206                                       const geometry_msgs::PoseStamped& goal){
00207   // Fill up statistics and publish
00208   sbpl_lattice_planner::SBPLLatticePlannerStats stats;
00209   stats.initial_epsilon = initial_epsilon_;
00210   stats.plan_to_first_solution = false;
00211   stats.final_number_of_expands = planner_->get_n_expands();
00212   stats.allocated_time = allocated_time_;
00213 
00214   stats.time_to_first_solution = planner_->get_initial_eps_planning_time();
00215   stats.actual_time = planner_->get_final_eps_planning_time();
00216   stats.number_of_expands_initial_solution = planner_->get_n_expands_init_solution();
00217   stats.final_epsilon = planner_->get_final_epsilon();
00218 
00219   stats.solution_cost = solution_cost;
00220   stats.path_size = solution_size;
00221   stats.start = start;
00222   stats.goal = goal;
00223   stats_publisher_.publish(stats);
00224 }
00225 
00226 bool SBPLLatticePlanner::makePlan(const geometry_msgs::PoseStamped& start,
00227                                  const geometry_msgs::PoseStamped& goal,
00228                                  std::vector<geometry_msgs::PoseStamped>& plan){
00229   if(!initialized_){
00230     ROS_ERROR("Global planner is not initialized");
00231     return false;
00232   }
00233 
00234   plan.clear();
00235 
00236   ROS_DEBUG("[sbpl_lattice_planner] getting fresh copy of costmap");
00237   costmap_ros_->clearRobotFootprint();
00238   ROS_DEBUG("[sbpl_lattice_planner] robot footprint cleared");
00239 
00240   costmap_ros_->getCostmapCopy(cost_map_);
00241 
00242   ROS_INFO("[sbpl_lattice_planner] getting start point (%g,%g) goal point (%g,%g)",
00243            start.pose.position.x, start.pose.position.y,goal.pose.position.x, goal.pose.position.y);
00244   double theta_start = 2 * atan2(start.pose.orientation.z, start.pose.orientation.w);
00245   double theta_goal = 2 * atan2(goal.pose.orientation.z, goal.pose.orientation.w);
00246 
00247   try{
00248     int ret = env_->SetStart(start.pose.position.x - cost_map_.getOriginX(), start.pose.position.y - cost_map_.getOriginY(), theta_start);
00249     if(ret < 0 || planner_->set_start(ret) == 0){
00250       ROS_ERROR("ERROR: failed to set start state\n");
00251       return false;
00252     }
00253   }
00254   catch(SBPL_Exception e){
00255     ROS_ERROR("SBPL encountered a fatal exception while setting the start state");
00256     return false;
00257   }
00258 
00259   try{
00260     int ret = env_->SetGoal(goal.pose.position.x - cost_map_.getOriginX(), goal.pose.position.y - cost_map_.getOriginY(), theta_goal);
00261     if(ret < 0 || planner_->set_goal(ret) == 0){
00262       ROS_ERROR("ERROR: failed to set goal state\n");
00263       return false;
00264     }
00265   }
00266   catch(SBPL_Exception e){
00267     ROS_ERROR("SBPL encountered a fatal exception while setting the goal state");
00268     return false;
00269   }
00270   
00271   int offOnCount = 0;
00272   int onOffCount = 0;
00273   int allCount = 0;
00274   vector<nav2dcell_t> changedcellsV;
00275 
00276   for(unsigned int ix = 0; ix < cost_map_.getSizeInCellsX(); ix++) {
00277     for(unsigned int iy = 0; iy < cost_map_.getSizeInCellsY(); iy++) {
00278 
00279       unsigned char oldCost = env_->GetMapCost(ix,iy);
00280       unsigned char newCost = costMapCostToSBPLCost(cost_map_.getCost(ix,iy));
00281 
00282       if(oldCost == newCost) continue;
00283 
00284       allCount++;
00285 
00286       //first case - off cell goes on
00287 
00288       if((oldCost != costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) && oldCost != costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE)) &&
00289           (newCost == costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) || newCost == costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE))) {
00290         offOnCount++;
00291       }
00292 
00293       if((oldCost == costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) || oldCost == costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE)) &&
00294           (newCost != costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) && newCost != costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE))) {
00295         onOffCount++;
00296       }
00297       env_->UpdateCost(ix, iy, costMapCostToSBPLCost(cost_map_.getCost(ix,iy)));
00298 
00299       nav2dcell_t nav2dcell;
00300       nav2dcell.x = ix;
00301       nav2dcell.y = iy;
00302       changedcellsV.push_back(nav2dcell);
00303     }
00304   }
00305 
00306   try{
00307     if(!changedcellsV.empty()){
00308       StateChangeQuery* scq = new LatticeSCQ(env_, changedcellsV);
00309       planner_->costs_changed(*scq);
00310       delete scq;
00311     }
00312 
00313     if(allCount > force_scratch_limit_)
00314       planner_->force_planning_from_scratch();
00315   }
00316   catch(SBPL_Exception e){
00317     ROS_ERROR("SBPL failed to update the costmap");
00318     return false;
00319   }
00320 
00321   //setting planner parameters
00322   ROS_DEBUG("allocated:%f, init eps:%f\n",allocated_time_,initial_epsilon_);
00323   planner_->set_initialsolution_eps(initial_epsilon_);
00324   planner_->set_search_mode(false);
00325 
00326   ROS_DEBUG("[sbpl_lattice_planner] run planner");
00327   vector<int> solution_stateIDs;
00328   int solution_cost;
00329   try{
00330     int ret = planner_->replan(allocated_time_, &solution_stateIDs, &solution_cost);
00331     if(ret)
00332       ROS_DEBUG("Solution is found\n");
00333     else{
00334       ROS_INFO("Solution not found\n");
00335       publishStats(solution_cost, 0, start, goal);
00336       return false;
00337     }
00338   }
00339   catch(SBPL_Exception e){
00340     ROS_ERROR("SBPL encountered a fatal exception while planning");
00341     return false;
00342   }
00343 
00344   ROS_DEBUG("size of solution=%d", (int)solution_stateIDs.size());
00345 
00346   vector<EnvNAVXYTHETALAT3Dpt_t> sbpl_path;
00347   try{
00348     env_->ConvertStateIDPathintoXYThetaPath(&solution_stateIDs, &sbpl_path);
00349   }
00350   catch(SBPL_Exception e){
00351     ROS_ERROR("SBPL encountered a fatal exception while reconstructing the path");
00352     return false;
00353   }
00354   ROS_DEBUG("Plan has %d points.\n", (int)sbpl_path.size());
00355   ros::Time plan_time = ros::Time::now();
00356 
00357   //create a message for the plan 
00358   nav_msgs::Path gui_path;
00359   gui_path.set_poses_size(sbpl_path.size());
00360   gui_path.header.frame_id = costmap_ros_->getGlobalFrameID();
00361   gui_path.header.stamp = plan_time;
00362   for(unsigned int i=0; i<sbpl_path.size(); i++){
00363     geometry_msgs::PoseStamped pose;
00364     pose.header.stamp = plan_time;
00365     pose.header.frame_id = costmap_ros_->getGlobalFrameID();
00366 
00367     pose.pose.position.x = sbpl_path[i].x + cost_map_.getOriginX();
00368     pose.pose.position.y = sbpl_path[i].y + cost_map_.getOriginY();
00369     pose.pose.position.z = start.pose.position.z;
00370 
00371     btQuaternion temp;
00372     temp.setEulerZYX(sbpl_path[i].theta,0,0);
00373     pose.pose.orientation.x = temp.getX();
00374     pose.pose.orientation.y = temp.getY();
00375     pose.pose.orientation.z = temp.getZ();
00376     pose.pose.orientation.w = temp.getW();
00377 
00378     plan.push_back(pose);
00379 
00380     gui_path.poses[i].pose.position.x = plan[i].pose.position.x;
00381     gui_path.poses[i].pose.position.y = plan[i].pose.position.y;
00382     gui_path.poses[i].pose.position.z = plan[i].pose.position.z;
00383   }
00384   plan_pub_.publish(gui_path);
00385   publishStats(solution_cost, sbpl_path.size(), start, goal);
00386 
00387   return true;
00388 }
00389 };