29 #ifndef VIGIR_FOOTSTEP_PLANNING_OCCUPANCY_GRID_MAP_HEURISTIC_H__ 30 #define VIGIR_FOOTSTEP_PLANNING_OCCUPANCY_GRID_MAP_HEURISTIC_H__ 32 #include <boost/thread/mutex.hpp> 34 #include <nav_msgs/OccupancyGrid.h> 36 #include <vigir_footstep_planning_plugins/plugins/heuristic_plugin.h> 44 :
public HeuristicPlugin
49 bool loadParams(
const vigir_generic_params::ParameterSet& params = vigir_generic_params::ParameterSet())
override;
51 bool initialize(
const vigir_generic_params::ParameterSet& params = vigir_generic_params::ParameterSet())
override;
53 double getHeuristicValue(
const State& from,
const State& to,
const State& start,
const State& goal)
const override;
56 void mapCallback(
const nav_msgs::OccupancyGridConstPtr& occupancy_grid_map);
Stores a nav_msgs::OccupancyGrid in a convenient opencv cv::Mat as binary map (free: 255...