SelfLocalizer.h
Go to the documentation of this file.
00001 #ifndef SELF_LOCALIZER_H
00002 #define SELF_LOCALIZER_H
00003 
00004 #include <string>
00005 
00006 #include <ros/ros.h>
00007 #include <tf/transform_listener.h>
00008 #include <nav_msgs/OccupancyGrid.h>
00009 #include <geometry_msgs/PoseArray.h>
00010 #include <sensor_msgs/LaserScan.h>
00011 
00012 #include <nav2d_localizer/map.h>
00013 #include <nav2d_localizer/pf.h>
00014 #include <nav2d_localizer/pf_pdf.h>
00015 
00016 class OdometryData
00017 {
00018 public:
00019         OdometryData(const tf::StampedTransform& pNewPose, const tf::StampedTransform& pLastPose);
00020         
00021         double mDeltaX;
00022         double mDeltaY;
00023         double mDeltaTheta;
00024 };
00025 
00026 class LaserData
00027 {
00028 public:
00029         LaserData(const sensor_msgs::LaserScan::ConstPtr& scan);
00030         ~LaserData();
00031         
00032         int mRangeCount;
00033         double mRangeMax;
00034         double (*mRanges)[2];
00035 };
00036 
00037 class SelfLocalizer
00038 {
00039 public:
00040         SelfLocalizer(bool publish = true);
00041         ~SelfLocalizer();
00042         
00043         bool initialize();
00044         
00045         // Uniformly distribute particles in the map
00046         static pf_vector_t distributeParticles(void* map);
00047         
00048         // Create a map from a ROS OccupancyGrid
00049         void convertMap(const nav_msgs::OccupancyGrid& map_msg);
00050         
00051         void process(const sensor_msgs::LaserScan::ConstPtr& scan);
00052         
00053         double getCovariance();
00054         tf::Transform getBestPose();
00055 
00056         tf::StampedTransform getMapToOdometry()
00057         {
00058                 return tf::StampedTransform(mMapToOdometry, ros::Time::now(), mMapFrame, mOdometryFrame);
00059         }
00060         
00061         void publishParticleCloud();
00062         
00063         // These methods are static, so the particle filter can call them via callback.
00064         // The parameters are static, so they can be used within these two methods.
00065         static double calculateMoveModel(OdometryData* data, pf_sample_set_t* set);
00066         static double calculateBeamModel(LaserData *data, pf_sample_set_t* set);
00067         static double calculateLikelihoodFieldModel(LaserData *data, pf_sample_set_t* set);
00068 
00069 private:        
00070         // Parameters for movement model (I don't know what they do.)
00071         static double sAlpha1, sAlpha2, sAlpha3, sAlpha4;
00072         static pf_vector_t sLaserPose;
00073         
00074         // How many range readings from every scan are used for the sensor model update
00075         static int sMaxBeams;
00076         
00077         // Max distance at which we care about obstacles for constructing the likelihood field
00078         static double sLikelihoodMaxDist;
00079         
00080         // The map
00081         static map_t* sMap;
00082         
00083         // Stddev of Gaussian model for laser hits
00084         static double sSigmaHit;
00085         
00086         // Decay rate of exponential model for short readings
00087         static double sLamdaShort;
00088         
00089         // Mixture params for the components of the model, must sum to 1
00090         static double sZHit;
00091         static double sZShort;
00092         static double sZMax;
00093         static double sZRand;
00094         
00095         std::string mMapFrame;
00096         std::string mOdometryFrame;
00097         std::string mRobotFrame;
00098         std::string mLaserFrame;
00099         
00100 private:
00101         // The actual particle filter
00102         pf_t* mParticleFilter;
00103         int mProcessedScans;
00104         
00105         // ROS stuff
00106         tf::TransformListener mTransformListener;
00107         ros::Publisher mParticlePublisher;
00108         
00109         // The pose of the last scan used for localization
00110         static tf::StampedTransform mLastPose;
00111         
00112         // The current localization result
00113         tf::Transform mMapToOdometry;
00114         
00115         // Parameters
00116         int mMinParticles;
00117         int mMaxParticles;
00118         double mAlphaSlow;
00119         double mAlphaFast;
00120         double mPopulationErr;
00121         double mPopulationZ;    
00122         
00123         double mMinTranslation;
00124         double mMinRotation;
00125         
00126         bool mPublishParticles;
00127         bool mFirstScanReceived;
00128         
00129         int mLaserModelType; // 1 = beam, 2 = likelihood field
00130         
00131         pf_sensor_model_fn_t mSensorModelFunction;
00132         pf_action_model_fn_t mActionModelFunction;
00133 };
00134 
00135 #endif


nav2d_localizer
Author(s): Sebastian Kasperski
autogenerated on Thu Aug 27 2015 14:07:21