hot_map_heuristic.h
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2018, Alexander Stumpf, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Simulation, Systems Optimization and Robotics
00013 //       group, TU Darmstadt nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #ifndef VIGIR_FOOTSTEP_PLANNING_HOT_MAP_HEURISTIC_H__
00030 #define VIGIR_FOOTSTEP_PLANNING_HOT_MAP_HEURISTIC_H__
00031 
00032 #include <ros/ros.h>
00033 #include <boost/thread/mutex.hpp>
00034 
00035 #include <nav_msgs/OccupancyGrid.h>
00036 
00037 #include <vigir_footstep_planning_lib/math.h>
00038 
00039 #include <vigir_footstep_planning_plugins/plugins/heuristic_plugin.h>
00040 
00041 
00042 
00043 namespace vigir_footstep_planning
00044 {
00045 class HotMapHeuristic
00046   : public HeuristicPlugin
00047 {
00048 struct StateKey
00049 {
00050   StateKey(const State& s, double cell_size, double angle_bin_size)
00051     : x(state_2_cell(s.getX(), cell_size))
00052     , y(state_2_cell(s.getY(), cell_size))
00053     , yaw(angle_state_2_cell(s.getYaw(), angle_bin_size))
00054   {
00055   }
00056 
00057   bool operator<(const StateKey& key) const
00058   {
00059     if (x < key.x) return true;
00060     if (x > key.x) return false;
00061     if (y < key.y) return true;
00062     if (y > key.y) return false;
00063     if (yaw < key.yaw) return true;
00064     if (yaw > key.yaw) return false;
00065     return false;
00066   }
00067 
00068   int x;
00069   int y;
00070   int yaw;
00071 };
00072 
00073 public:
00074   HotMapHeuristic();
00075 
00076   bool loadParams(const vigir_generic_params::ParameterSet& params = vigir_generic_params::ParameterSet()) override;
00077 
00078   bool initialize(const vigir_generic_params::ParameterSet& params = vigir_generic_params::ParameterSet()) override;
00079 
00080   void reset();
00081 
00082   double getHeuristicValue(const State& from, const State& to, const State& start, const State& goal) const override;
00083 
00084   // typedefs
00085   typedef std::map<StateKey, unsigned int> HotMap;
00086 
00087 protected:
00088   void publishHotMap(const ros::TimerEvent& publish_timer) const;
00089 
00090   // publisher
00091   ros::Publisher hot_map_pub_;
00092 
00093   ros::Timer publish_timer_;
00094 
00095   // mutex
00096   mutable boost::shared_mutex hot_map_shared_mutex_;
00097 
00098   mutable HotMap hot_map_;
00099   mutable unsigned int total_hit_counter_;
00100 
00101   double cell_size_;
00102   double angle_bin_size_;
00103 };
00104 }
00105 
00106 #endif


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