PathCostHeuristic.cpp
Go to the documentation of this file.
00001 /*
00002  * A footstep planner for humanoid robots
00003  *
00004  * Copyright 2010-2011 Johannes Garimort, Armin Hornung, University of Freiburg
00005  * http://www.ros.org/wiki/footstep_planner
00006  *
00007  *
00008  * This program is free software: you can redistribute it and/or modify
00009  * it under the terms of the GNU General Public License as published by
00010  * the Free Software Foundation, version 3.
00011  *
00012  * This program is distributed in the hope that it will be useful,
00013  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015  * GNU General Public License for more details.
00016  *
00017  * You should have received a copy of the GNU General Public License
00018  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
00019  */
00020 
00021 #include <footstep_planner/PathCostHeuristic.h>
00022 
00023 
00024 namespace footstep_planner
00025 {
00026 PathCostHeuristic::PathCostHeuristic(double cell_size,
00027                                      int    num_angle_bins,
00028                                      double step_cost,
00029                                      double diff_angle_cost,
00030                                      double max_step_width,
00031                                      double inflation_radius)
00032 : Heuristic(cell_size, num_angle_bins, PATH_COST),
00033   ivpGrid(NULL),
00034   ivStepCost(step_cost),
00035   ivDiffAngleCost(diff_angle_cost),
00036   ivMaxStepWidth(max_step_width),
00037   ivInflationRadius(inflation_radius),
00038   ivGoalX(-1),
00039   ivGoalY(-1)
00040 {};
00041 
00042 
00043 PathCostHeuristic::~PathCostHeuristic()
00044 {
00045   if (ivpGrid)
00046     resetGrid();
00047 }
00048 
00049 
00050 double
00051 PathCostHeuristic::getHValue(const PlanningState& current,
00052                              const PlanningState& to)
00053 const
00054 {
00055   assert(ivGoalX >= 0 && ivGoalY >= 0);
00056 
00057   if (current == to)
00058     return 0.0;
00059 
00060   unsigned int from_x;
00061   unsigned int from_y;
00062   // could be removed after more testing (then use ...noBounds... again)
00063   ivMapPtr->worldToMapNoBounds(cell_2_state(current.getX(), ivCellSize),
00064                                cell_2_state(current.getY(), ivCellSize),
00065                                from_x, from_y);
00066 
00067   unsigned int to_x;
00068   unsigned int to_y;
00069   // could be removed after more testing (then use ...noBounds... again)
00070   ivMapPtr->worldToMapNoBounds(cell_2_state(to.getX(), ivCellSize),
00071                                cell_2_state(to.getY(), ivCellSize),
00072                                to_x, to_y);
00073 
00074   // cast to unsigned int is safe since ivGoalX/ivGoalY are checked to be >= 0
00075   if ((unsigned int)ivGoalX != to_x || (unsigned int)ivGoalY != to_y)
00076   {
00077     ROS_ERROR("PathCostHeuristic::getHValue to a different value than "
00078               "precomputed, heuristic values will be wrong. You need to call "
00079               "calculateDistances() before!");
00080   }
00081   assert((unsigned int)ivGoalX == to_x && (unsigned int)ivGoalY == to_y);
00082 
00083   double dist = double(ivGridSearchPtr->getlowerboundoncostfromstart_inmm(
00084       from_x, from_y)) / 1000.0;
00085 
00086   double expected_steps = dist / ivMaxStepWidth;
00087   double diff_angle = 0.0;
00088   if (ivDiffAngleCost > 0.0)
00089   {
00090     // get the number of bins between from.theta and to.theta
00091     int diff_angle_disc = (
00092         ((to.getTheta() - current.getTheta()) % ivNumAngleBins) +
00093         ivNumAngleBins) % ivNumAngleBins;
00094     // get the rotation independent from the rotation direction
00095     diff_angle = std::abs(angles::normalize_angle(
00096         angle_cell_2_state(diff_angle_disc, ivNumAngleBins)));
00097   }
00098 
00099   return (dist + expected_steps * ivStepCost + diff_angle * ivDiffAngleCost);
00100 }
00101 
00102 
00103 bool
00104 PathCostHeuristic::calculateDistances(const PlanningState& from,
00105                                       const PlanningState& to)
00106 {
00107   assert(ivMapPtr);
00108 
00109   unsigned int from_x;
00110   unsigned int from_y;
00111   ivMapPtr->worldToMapNoBounds(cell_2_state(from.getX(), ivCellSize),
00112                                cell_2_state(from.getY(), ivCellSize),
00113                                from_x, from_y);
00114 
00115   unsigned int to_x;
00116   unsigned int to_y;
00117   ivMapPtr->worldToMapNoBounds(cell_2_state(to.getX(), ivCellSize),
00118                                cell_2_state(to.getY(), ivCellSize),
00119                                to_x, to_y);
00120 
00121   if ((int)to_x != ivGoalX || (int)to_y != ivGoalY)
00122   {
00123     ivGoalX = to_x;
00124     ivGoalY = to_y;
00125     ivGridSearchPtr->search(ivpGrid, cvObstacleThreshold,
00126                             ivGoalX, ivGoalY, from_x, from_y,
00127                             SBPL_2DGRIDSEARCH_TERM_CONDITION_ALLCELLS);
00128   }
00129 
00130   return true;
00131 }
00132 
00133 
00134 void
00135 PathCostHeuristic::updateMap(gridmap_2d::GridMap2DPtr map)
00136 {
00137   ivMapPtr.reset();
00138   ivMapPtr = map;
00139 
00140   ivGoalX = ivGoalY = -1;
00141 
00142   unsigned width = ivMapPtr->getInfo().width;
00143   unsigned height = ivMapPtr->getInfo().height;
00144 
00145   if (ivGridSearchPtr)
00146     ivGridSearchPtr->destroy();
00147   ivGridSearchPtr.reset(new SBPL2DGridSearch(width, height,
00148                                              ivMapPtr->getResolution()));
00149   if (ivpGrid)
00150     resetGrid();
00151   ivpGrid = new unsigned char* [width];
00152 
00153   for (unsigned x = 0; x < width; ++x)
00154     ivpGrid[x] = new unsigned char [height];
00155   for (unsigned y = 0; y < height; ++y)
00156   {
00157     for (unsigned x = 0; x < width; ++x)
00158     {
00159       float dist = ivMapPtr->distanceMapAtCell(x,y);
00160       if (dist < 0.0f)
00161         ROS_ERROR("Distance map at %d %d out of bounds", x, y);
00162       else if (dist <= ivInflationRadius)
00163         ivpGrid[x][y] = 255;
00164       else
00165         ivpGrid[x][y] = 0;
00166     }
00167   }
00168 }
00169 
00170 
00171 void
00172 PathCostHeuristic::resetGrid()
00173 {
00174   CvSize size = ivMapPtr->size();
00175   for (int x = 0; x < size.width; ++x)
00176   {
00177     if (ivpGrid[x])
00178     {
00179       delete[] ivpGrid[x];
00180       ivpGrid[x] = NULL;
00181     }
00182   }
00183   delete[] ivpGrid;
00184   ivpGrid = NULL;
00185 }
00186 } // end of namespace


footstep_planner
Author(s): Johannes Garimort, Armin Hornung
autogenerated on Wed Aug 26 2015 11:54:32