00001 // SVN $HeadURL$ 00002 // SVN $Id$ 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_ENDPOINTMODEL_H_ 00025 #define HUMANOID_LOCALIZATION_ENDPOINTMODEL_H_ 00026 00027 #include <limits> 00028 #include<cmath> 00029 00030 #include <omp.h> 00031 00032 #include <ros/ros.h> 00033 #include <tf/transform_datatypes.h> 00034 00035 #include <humanoid_localization/ObservationModel.h> 00036 #include <octomap/octomap.h> 00037 #include <dynamicEDT3D/dynamicEDTOctomap.h> 00038 #include <visualization_msgs/Marker.h> 00039 00040 00041 namespace humanoid_localization{ 00042 class EndpointModel : public ObservationModel { 00043 public: 00044 EndpointModel(ros::NodeHandle* nh, boost::shared_ptr<MapModel> mapModel, EngineT * rngEngine); 00045 virtual ~EndpointModel(); 00046 virtual void integrateMeasurement(Particles& particles, const PointCloud& pc, const std::vector<float>& ranges, float max_range, const tf::Transform& baseToSensor); 00047 00048 virtual void setMap(boost::shared_ptr<octomap::OcTree> map); 00049 00050 protected: 00051 bool getHeightError(const Particle& p, const tf::StampedTransform& footprintToBase, double& heightError) const; 00052 void initDistanceMap(); 00053 double m_sigma; 00054 double m_maxObstacleDistance; 00055 boost::shared_ptr<DynamicEDTOctomap> m_distanceMap; 00056 }; 00057 00058 } 00059 00060 #endif