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 #ifndef FOOTSTEP_PLANNER_PATHCOSTHEURISTIC_H_ 00022 #define FOOTSTEP_PLANNER_PATHCOSTHEURISTIC_H_ 00023 00024 #include <footstep_planner/Heuristic.h> 00025 #include <gridmap_2d/GridMap2D.h> 00026 #include <sbpl/headers.h> 00027 00028 00029 namespace footstep_planner 00030 { 00045 class PathCostHeuristic : public Heuristic 00046 { 00047 public: 00048 PathCostHeuristic(double cell_size, int num_angle_bins, 00049 double step_cost, double diff_angle_cost, 00050 double max_step_width, double inflation_radius); 00051 virtual ~PathCostHeuristic(); 00052 00057 virtual double getHValue(const PlanningState& current, 00058 const PlanningState& to) const; 00059 00066 bool calculateDistances(const PlanningState& from, const PlanningState& to); 00067 00068 void updateMap(gridmap_2d::GridMap2DPtr map); 00069 00070 private: 00071 static const int cvObstacleThreshold = 200; 00072 00073 unsigned char** ivpGrid; 00074 00075 double ivStepCost; 00076 double ivDiffAngleCost; 00077 double ivMaxStepWidth; 00078 double ivInflationRadius; 00079 00080 int ivGoalX; 00081 int ivGoalY; 00082 00083 gridmap_2d::GridMap2DPtr ivMapPtr; 00084 boost::shared_ptr<SBPL2DGridSearch> ivGridSearchPtr; 00085 00086 void resetGrid(); 00087 }; 00088 } 00089 #endif // FOOTSTEP_PLANNER_PATHCOSTHEURISTIC_H_