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