$search
Determining the heuristic value by calculating a 2D path from each grid cell of the map to the goal and using the path length as expected distance. More...
#include <PathCostHeuristic.h>
Public Member Functions | |
bool | calculateDistances (const PlanningState &from, const PlanningState &to) |
Calculates for each grid cell of the map a 2D path to the cell (to.x, to.y). For forward planning 'to' is supposed to be the goal state, for backward planning 'to' is supposed to be the start state. | |
virtual double | getHValue (const PlanningState ¤t, const PlanningState &to) const |
PathCostHeuristic (double cell_size, int num_angle_bins, double step_cost, double diff_angle_cost, double max_step_width, double inflation_radius) | |
void | updateMap (gridmap_2d::GridMap2DPtr map) |
virtual | ~PathCostHeuristic () |
Private Member Functions | |
void | resetGrid () |
Private Attributes | |
double | ivDiffAngleCost |
int | ivGoalX |
int | ivGoalY |
boost::shared_ptr < SBPL2DGridSearch > | ivGridSearchPtr |
double | ivInflationRadius |
gridmap_2d::GridMap2DPtr | ivMapPtr |
double | ivMaxStepWidth |
unsigned char ** | ivpGrid |
double | ivStepCost |
Static Private Attributes | |
static const int | cvObstacleThreshold = 200 |
Determining the heuristic value by calculating a 2D path from each grid cell of the map to the goal and using the path length as expected distance.
The heuristic value consists of the following factors:
+ The expected distance retreived from the 2D path.
+ The expected path costs.
+ The difference between the orientation of the two states multiplied by some cost factor.
Definition at line 48 of file PathCostHeuristic.h.
footstep_planner::PathCostHeuristic::PathCostHeuristic | ( | double | cell_size, | |
int | num_angle_bins, | |||
double | step_cost, | |||
double | diff_angle_cost, | |||
double | max_step_width, | |||
double | inflation_radius | |||
) |
Definition at line 29 of file PathCostHeuristic.cpp.
footstep_planner::PathCostHeuristic::~PathCostHeuristic | ( | ) | [virtual] |
Definition at line 46 of file PathCostHeuristic.cpp.
bool footstep_planner::PathCostHeuristic::calculateDistances | ( | const PlanningState & | from, | |
const PlanningState & | to | |||
) |
Calculates for each grid cell of the map a 2D path to the cell (to.x, to.y). For forward planning 'to' is supposed to be the goal state, for backward planning 'to' is supposed to be the start state.
Definition at line 107 of file PathCostHeuristic.cpp.
double footstep_planner::PathCostHeuristic::getHValue | ( | const PlanningState & | current, | |
const PlanningState & | to | |||
) | const [virtual] |
Implements footstep_planner::Heuristic.
Definition at line 54 of file PathCostHeuristic.cpp.
void footstep_planner::PathCostHeuristic::resetGrid | ( | ) | [private] |
Definition at line 175 of file PathCostHeuristic.cpp.
void footstep_planner::PathCostHeuristic::updateMap | ( | gridmap_2d::GridMap2DPtr | map | ) |
Definition at line 138 of file PathCostHeuristic.cpp.
const int footstep_planner::PathCostHeuristic::cvObstacleThreshold = 200 [static, private] |
Definition at line 74 of file PathCostHeuristic.h.
double footstep_planner::PathCostHeuristic::ivDiffAngleCost [private] |
Definition at line 79 of file PathCostHeuristic.h.
int footstep_planner::PathCostHeuristic::ivGoalX [private] |
Definition at line 83 of file PathCostHeuristic.h.
int footstep_planner::PathCostHeuristic::ivGoalY [private] |
Definition at line 84 of file PathCostHeuristic.h.
boost::shared_ptr<SBPL2DGridSearch> footstep_planner::PathCostHeuristic::ivGridSearchPtr [private] |
Definition at line 87 of file PathCostHeuristic.h.
double footstep_planner::PathCostHeuristic::ivInflationRadius [private] |
Definition at line 81 of file PathCostHeuristic.h.
Definition at line 86 of file PathCostHeuristic.h.
double footstep_planner::PathCostHeuristic::ivMaxStepWidth [private] |
Definition at line 80 of file PathCostHeuristic.h.
unsigned char** footstep_planner::PathCostHeuristic::ivpGrid [private] |
Definition at line 76 of file PathCostHeuristic.h.
double footstep_planner::PathCostHeuristic::ivStepCost [private] |
Definition at line 78 of file PathCostHeuristic.h.