hot_map_heuristic.cpp
Go to the documentation of this file.
00001 #include <vigir_footstep_planning_default_plugins/heuristics/hot_map_heuristic.h>
00002 
00003 
00004 
00005 namespace vigir_footstep_planning
00006 {
00007 HotMapHeuristic::HotMapHeuristic()
00008 : HeuristicPlugin("hot_map_heuristic")
00009 {
00010 }
00011 
00012 bool HotMapHeuristic::loadParams(const vigir_generic_params::ParameterSet& params)
00013 {
00014   if (!HeuristicPlugin::loadParams(params))
00015     return false;
00016 
00017   params.getParam("collision_check/cell_size", cell_size_);
00018   int num_angle_bins;
00019   params.getParam("collision_check/num_angle_bins", num_angle_bins);
00020   angle_bin_size_ = 2.0*M_PI / static_cast<double>(num_angle_bins);
00021   return true;
00022 }
00023 
00024 bool HotMapHeuristic::initialize(const vigir_generic_params::ParameterSet& params)
00025 {
00026   if (!HeuristicPlugin::initialize(params))
00027     return false;
00028 
00029   // publish topics
00030   hot_map_pub_ = nh_.advertise<nav_msgs::OccupancyGrid>("hot_map", 1);
00031   publish_timer_ = nh_.createTimer(ros::Duration(1.0), &HotMapHeuristic::publishHotMap, this);
00032 
00033   return true;
00034 }
00035 
00036 void HotMapHeuristic::reset()
00037 {
00038   boost::unique_lock<boost::shared_mutex> lock(hot_map_shared_mutex_);
00039   hot_map_.clear();
00040   total_hit_counter_ = 0;
00041 }
00042 
00043 double HotMapHeuristic::getHeuristicValue(const State& from, const State& to, const State& /*start*/, const State& /*goal*/) const
00044 {
00045   if (from == to)
00046     return 0.0;
00047 
00048   unsigned int hit = 0;
00049   {
00050     boost::unique_lock<boost::shared_mutex> lock(hot_map_shared_mutex_);
00051     hit = hot_map_[StateKey(from, cell_size_, angle_bin_size_)]++;
00052     total_hit_counter_++;
00053   }
00054 
00055   return 1000.0 * static_cast<double>(hit)/static_cast<double>(total_hit_counter_);
00056 }
00057 
00058 void HotMapHeuristic::publishHotMap(const ros::TimerEvent& /*publish_timer*/) const
00059 {
00060   if (hot_map_pub_.getNumSubscribers() > 0)
00061   {
00062     boost::shared_lock<boost::shared_mutex> lock(hot_map_shared_mutex_);
00063 
00064     nav_msgs::OccupancyGrid grid_map;
00065 
00066     grid_map.header.stamp = ros::Time::now();
00067     grid_map.header.frame_id = "/world";
00068 
00070 
00071     hot_map_pub_.publish(grid_map);
00072   }
00073 }
00074 }
00075 
00076 #include <pluginlib/class_list_macros.h>
00077 PLUGINLIB_EXPORT_CLASS(vigir_footstep_planning::HotMapHeuristic, vigir_footstep_planning::HeuristicPlugin)


vigir_footstep_planning_default_plugins
Author(s): Alexander Stumpf
autogenerated on Thu Jun 6 2019 18:38:06