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


footstep_planner
Author(s): Johannes Garimort, Armin Hornung
autogenerated on Tue Oct 15 2013 10:06:52