00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
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
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
00073 ivMapPtr->worldToMapNoBounds(cell_2_state(to.getX(), ivCellSize),
00074 cell_2_state(to.getY(), ivCellSize),
00075 to_x, to_y);
00076
00077
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
00094 int diff_angle_disc = (
00095 ((to.getTheta() - current.getTheta()) % ivNumAngleBins) +
00096 ivNumAngleBins) % ivNumAngleBins;
00097
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 }