00001 #include <vigir_footstep_planning_default_plugins/heuristics/occupancy_grid_map_heuristic.h> 00002 00003 00004 00005 namespace vigir_footstep_planning 00006 { 00007 OccupancyGridMapHeuristic::OccupancyGridMapHeuristic() 00008 : HeuristicPlugin("occupancy_grid_map_heuristic") 00009 { 00010 } 00011 00012 bool OccupancyGridMapHeuristic::loadParams(const vigir_generic_params::ParameterSet& params) 00013 { 00014 if (!HeuristicPlugin::loadParams(params)) 00015 return false; 00016 00017 getParam("grid_map_topic", grid_map_topic_, std::string()); 00018 return true; 00019 } 00020 00021 bool OccupancyGridMapHeuristic::initialize(const vigir_generic_params::ParameterSet& params) 00022 { 00023 if (!HeuristicPlugin::initialize(params)) 00024 return false; 00025 00026 // subscribe topics 00027 occupancy_grid_map_sub_ = nh_.subscribe<nav_msgs::OccupancyGrid>(grid_map_topic_, 1, &OccupancyGridMapHeuristic::mapCallback, this); 00028 00029 return true; 00030 } 00031 00032 void OccupancyGridMapHeuristic::mapCallback(const nav_msgs::OccupancyGridConstPtr& occupancy_grid_map) 00033 { 00034 boost::unique_lock<boost::shared_mutex> lock(grid_map_shared_mutex_); 00035 distance_map_.setMap(occupancy_grid_map); 00036 } 00037 00038 double OccupancyGridMapHeuristic::getHeuristicValue(const State& from, const State& to, const State& /*start*/, const State& /*goal*/) const 00039 { 00040 if (from == to) 00041 return 0.0; 00042 00043 boost::shared_lock<boost::shared_mutex> lock(grid_map_shared_mutex_); 00044 00045 double d = distance_map_.distanceMapAt(from.getX(), from.getY()); 00046 if (d < 0.0) 00047 return 0.0; 00048 00049 if (d < 0.01) 00050 return max_heuristic_value_; 00051 00052 return 5.0*std::max(0.0, 0.5-d); 00053 } 00054 } 00055 00056 #include <pluginlib/class_list_macros.h> 00057 PLUGINLIB_EXPORT_CLASS(vigir_footstep_planning::OccupancyGridMapHeuristic, vigir_footstep_planning::HeuristicPlugin)