humanoid_localization: humanoid_localization: MapModel.h Source File

00001 // SVN $HeadURL: http://alufr-ros-pkg.googlecode.com/svn/trunk/humanoid_stacks/humanoid_navigation/humanoid_localization/include/humanoid_localization/MapModel.h $
00002 // SVN $Id: MapModel.h 3860 2013-02-06 16:58:53Z hornunga@informatik.uni-freiburg.de $
00003 
00004 /*
00005  * 6D localization for humanoid robots
00006  *
00007  * Copyright 2009-2012 Armin Hornung, University of Freiburg
00008  * http://www.ros.org/wiki/humanoid_localization
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 HUMANOID_LOCALIZATION_MAPMODEL_H_
00025 #define HUMANOID_LOCALIZATION_MAPMODEL_H_
00026 
00027 #include <omp.h>
00028 #include <ros/ros.h>
00029 #include <octomap_msgs/conversions.h>
00030 #include <octomap_msgs/GetOctomap.h>
00031 #include <octomap_ros/conversions.h>
00032 
00033 #include <humanoid_localization/humanoid_localization_defs.h>
00034 
00035 namespace humanoid_localization{
00036 
00037 class MapModel{
00038 public:
00039   MapModel(ros::NodeHandle* nh);
00040   virtual ~MapModel();
00041 
00042   //void setMap(boost::shared_ptr<octomap::OcTree> map);
00043   boost::shared_ptr<octomap::OcTree> getMap() const;
00044 
00050   virtual void verifyPoses(Particles& particles);
00051 
00052   virtual void initGlobal(Particles& particles,
00053                           const Vector6d& initPose, const Vector6d& initNoise,
00054                           UniformGeneratorT& rngUniform, NormalGeneratorT& rngNormal);
00055 
00058   virtual bool isOccupied(const octomap::point3d& position) const;
00059   virtual bool isOccupied(octomap::OcTreeNode* node) const = 0;
00060   virtual double getFloorHeight(const tf::Transform& pose) const = 0;
00061 
00070   void getHeightlist(double x, double y, double totalHeight, std::vector<double>& heights);
00071 
00072 
00073 protected:
00074   boost::shared_ptr<octomap::OcTree> m_map;
00075 
00076   double m_motionMeanZ;
00077   double m_motionRangeZ;
00078   double m_motionRangeRoll;
00079   double m_motionRangePitch;
00080   double m_motionObstacleDist;
00081 
00082 };
00083 
00084 
00085 class DistanceMap : public MapModel{
00086 public:
00087   DistanceMap(ros::NodeHandle* nh);
00088   virtual ~DistanceMap();
00089 
00090   virtual bool isOccupied(octomap::OcTreeNode* node) const;
00091   virtual double getFloorHeight(const tf::Transform& pose) const;
00092 
00093 };
00094 
00095 
00096 class OccupancyMap : public MapModel{
00097 public:
00098   OccupancyMap(ros::NodeHandle* nh);
00099   virtual ~OccupancyMap();
00100 
00101   virtual bool isOccupied(octomap::OcTreeNode* node) const;
00102   virtual double getFloorHeight(const tf::Transform& pose) const;
00103 };
00104 
00105 }
00106 #endif