path_cost_heuristic.cpp
Go to the documentation of this file.
00001 #include <vigir_footstep_planning_default_plugins/heuristics/path_cost_heuristic.h>
00002 
00003 #include <pluginlib/class_list_macros.h>
00004 
00005 
00006 
00007 namespace vigir_footstep_planning
00008 {
00009 PathCostHeuristic::PathCostHeuristic()
00010   : HeuristicPlugin("path_cost_heuristic")
00011   , ivpGrid(NULL)
00012   , ivGoalX(-1)
00013   , ivGoalY(-1)
00014 {
00015 }
00016 
00017 PathCostHeuristic::~PathCostHeuristic()
00018 {
00019   if (ivpGrid)
00020     resetGrid();
00021 }
00022 
00023 bool PathCostHeuristic::loadParams(const vigir_generic_params::ParameterSet& params)
00024 {
00025   if (!HeuristicPlugin::loadParams(params))
00026     return false;
00027 
00028   params.getParam("collision_check/cell_size", ivCellSize);
00029   params.getParam("collision_check/num_angle_bins", ivNumAngleBins);
00030   ivAngleBinSize = 2.0*M_PI / static_cast<double>(ivNumAngleBins);
00031 
00032   params.getParam("const_step_cost_estimator/step_cost", ivStepCost, 0.1);
00033   params.getParam("diff_angle_cost", ivDiffAngleCost);
00034   params.getParam("max_step_dist/x", ivMaxStepWidth);
00037   return true;
00038 }
00039 
00040 double PathCostHeuristic::getHeuristicValue(const State& from, const State& to, const State& /*start*/, const State& /*goal*/) const
00041 {
00042   if (from == to)
00043     return 0.0;
00044 
00045   assert(ivGoalX >= 0 && ivGoalY >= 0);
00046 
00047   if (from == to)
00048     return 0.0;
00049 
00050   unsigned int from_x;
00051   unsigned int from_y;
00052   // could be removed after more testing (then use ...noBounds... again)
00053   ivMapPtr->worldToMapNoBounds(cell_2_state(from.getX(), ivCellSize),
00054                                cell_2_state(from.getY(), ivCellSize),
00055                                from_x, from_y);
00056 
00057   unsigned int to_x;
00058   unsigned int to_y;
00059   // could be removed after more testing (then use ...noBounds... again)
00060   ivMapPtr->worldToMapNoBounds(cell_2_state(to.getX(), ivCellSize),
00061                                cell_2_state(to.getY(), ivCellSize),
00062                                to_x, to_y);
00063 
00064   // cast to unsigned int is safe since ivGoalX/ivGoalY are checked to be >= 0
00065   if ((unsigned int)ivGoalX != to_x || (unsigned int)ivGoalY != to_y)
00066   {
00067     ROS_ERROR("PathCostHeuristic::getHValue to a different value than "
00068               "precomputed, heuristic values will be wrong. You need to call "
00069               "calculateDistances() before!");
00070   }
00071   assert((unsigned int)ivGoalX == to_x && (unsigned int)ivGoalY == to_y);
00072 
00073   double dist = double(ivGridSearchPtr->getlowerboundoncostfromstart_inmm(from_x, from_y)) / 1000.0;
00074 
00075   double expected_steps = dist / ivMaxStepWidth;
00076   double diff_angle = 0.0;
00077   if (ivDiffAngleCost > 0.0)
00078     diff_angle = std::abs(angles::shortest_angular_distance(to.getYaw(), from.getYaw()));
00079 
00080   return dist + expected_steps * ivStepCost + diff_angle * ivDiffAngleCost;
00081 }
00082 
00083 bool PathCostHeuristic::calculateDistances(const State& from, const State& to)
00084 {
00085   assert(ivMapPtr);
00086 
00087   unsigned int from_x;
00088   unsigned int from_y;
00089   ivMapPtr->worldToMapNoBounds(cell_2_state(from.getX(), ivCellSize),
00090                                cell_2_state(from.getY(), ivCellSize),
00091                                from_x, from_y);
00092 
00093   unsigned int to_x;
00094   unsigned int to_y;
00095   ivMapPtr->worldToMapNoBounds(cell_2_state(to.getX(), ivCellSize),
00096                                cell_2_state(to.getY(), ivCellSize),
00097                                to_x, to_y);
00098 
00099   if ((int)to_x != ivGoalX || (int)to_y != ivGoalY)
00100   {
00101     ivGoalX = to_x;
00102     ivGoalY = to_y;
00103     ivGridSearchPtr->search(ivpGrid, cvObstacleThreshold,
00104                             ivGoalX, ivGoalY, from_x, from_y,
00105                             SBPL_2DGRIDSEARCH_TERM_CONDITION_ALLCELLS);
00106   }
00107 
00108   return true;
00109 }
00110 
00111 void PathCostHeuristic::updateMap(vigir_gridmap_2d::GridMap2DPtr map)
00112 {
00113   ivMapPtr.reset();
00114   ivMapPtr = map;
00115 
00116   ivGoalX = ivGoalY = -1;
00117 
00118   unsigned width = ivMapPtr->getInfo().width;
00119   unsigned height = ivMapPtr->getInfo().height;
00120 
00121   if (ivGridSearchPtr)
00122     ivGridSearchPtr->destroy();
00123   ivGridSearchPtr.reset(new SBPL2DGridSearch(width, height,
00124                                              ivMapPtr->getResolution()));
00125   if (ivpGrid)
00126     resetGrid();
00127   ivpGrid = new unsigned char* [width];
00128 
00129   for (unsigned x = 0; x < width; ++x)
00130     ivpGrid[x] = new unsigned char [height];
00131   for (unsigned y = 0; y < height; ++y)
00132   {
00133     for (unsigned x = 0; x < width; ++x)
00134     {
00135       float dist = ivMapPtr->distanceMapAtCell(x,y);
00136       if (dist < 0.0f)
00137         ROS_ERROR("Distance map at %d %d out of bounds", x, y);
00138       else if (dist <= ivInflationRadius)
00139         ivpGrid[x][y] = 255;
00140       else
00141         ivpGrid[x][y] = 0;
00142     }
00143   }
00144 }
00145 
00146 void PathCostHeuristic::resetGrid()
00147 {
00148   CvSize size = ivMapPtr->size();
00149   for (int x = 0; x < size.width; ++x)
00150   {
00151     if (ivpGrid[x])
00152     {
00153       delete[] ivpGrid[x];
00154       ivpGrid[x] = NULL;
00155     }
00156   }
00157   delete[] ivpGrid;
00158   ivpGrid = NULL;
00159 }
00160 }
00161 
00162 PLUGINLIB_EXPORT_CLASS(vigir_footstep_planning::PathCostHeuristic, vigir_footstep_planning::HeuristicPlugin)


vigir_footstep_planning_default_plugins
Author(s): Alexander Stumpf
autogenerated on Fri Apr 7 2017 02:59:40