SelfLocalizer.h
Go to the documentation of this file.
1 #ifndef SELF_LOCALIZER_H
2 #define SELF_LOCALIZER_H
3 
4 #include <string>
5 
6 #include <ros/ros.h>
8 #include <nav_msgs/OccupancyGrid.h>
9 #include <geometry_msgs/PoseArray.h>
10 #include <sensor_msgs/LaserScan.h>
11 
12 #include <nav2d_localizer/map.h>
13 #include <nav2d_localizer/pf.h>
14 #include <nav2d_localizer/pf_pdf.h>
15 
17 {
18 public:
19  OdometryData(const tf::StampedTransform& pNewPose, const tf::StampedTransform& pLastPose);
20 
21  double mDeltaX;
22  double mDeltaY;
23  double mDeltaTheta;
24 };
25 
26 class LaserData
27 {
28 public:
29  LaserData(const sensor_msgs::LaserScan::ConstPtr& scan);
30  ~LaserData();
31 
33  double mRangeMax;
34  double (*mRanges)[2];
35 };
36 
38 {
39 public:
40  SelfLocalizer(bool publish = true);
41  ~SelfLocalizer();
42 
43  bool initialize();
44 
45  // Uniformly distribute particles in the map
46  static pf_vector_t distributeParticles(void* map);
47 
48  // Create a map from a ROS OccupancyGrid
49  void convertMap(const nav_msgs::OccupancyGrid& map_msg);
50 
51  void process(const sensor_msgs::LaserScan::ConstPtr& scan);
52 
53  double getCovariance();
54  tf::Transform getBestPose();
55 
57  {
58  return tf::StampedTransform(mMapToOdometry, ros::Time::now(), mMapFrame, mOdometryFrame);
59  }
60 
61  void publishParticleCloud();
62 
63  // These methods are static, so the particle filter can call them via callback.
64  // The parameters are static, so they can be used within these two methods.
65  static double calculateMoveModel(OdometryData* data, pf_sample_set_t* set);
66  static double calculateBeamModel(LaserData *data, pf_sample_set_t* set);
67  static double calculateLikelihoodFieldModel(LaserData *data, pf_sample_set_t* set);
68 
69 private:
70  // Parameters for movement model (I don't know what they do.)
71  static double sAlpha1, sAlpha2, sAlpha3, sAlpha4;
73 
74  // How many range readings from every scan are used for the sensor model update
75  static int sMaxBeams;
76 
77  // Max distance at which we care about obstacles for constructing the likelihood field
78  static double sLikelihoodMaxDist;
79 
80  // The map
81  static map_t* sMap;
82 
83  // Stddev of Gaussian model for laser hits
84  static double sSigmaHit;
85 
86  // Decay rate of exponential model for short readings
87  static double sLamdaShort;
88 
89  // Mixture params for the components of the model, must sum to 1
90  static double sZHit;
91  static double sZShort;
92  static double sZMax;
93  static double sZRand;
94 
95  std::string mMapFrame;
96  std::string mOdometryFrame;
97  std::string mRobotFrame;
98  std::string mLaserFrame;
99 
100 private:
101  // The actual particle filter
104 
105  // ROS stuff
108 
109  // The pose of the last scan used for localization
111 
112  // The current localization result
114 
115  // Parameters
118  double mAlphaSlow;
119  double mAlphaFast;
121  double mPopulationZ;
122 
124  double mMinRotation;
125 
128 
129  int mLaserModelType; // 1 = beam, 2 = likelihood field
130 
133 };
134 
135 #endif
OdometryData(const tf::StampedTransform &pNewPose, const tf::StampedTransform &pLastPose)
double(* pf_sensor_model_fn_t)(void *sensor_data, struct _pf_sample_set_t *set)
Definition: pf.h:54
static double sZHit
Definition: SelfLocalizer.h:90
double mDeltaX
Definition: SelfLocalizer.h:21
pf_action_model_fn_t mActionModelFunction
double mMinRotation
static pf_vector_t sLaserPose
Definition: SelfLocalizer.h:72
Definition: pf.h:111
ROSCONSOLE_DECL void initialize()
std::string mLaserFrame
Definition: SelfLocalizer.h:98
static double sZShort
Definition: SelfLocalizer.h:91
double mDeltaTheta
Definition: SelfLocalizer.h:23
tf::StampedTransform getMapToOdometry()
Definition: SelfLocalizer.h:56
double mMinTranslation
double mDeltaY
Definition: SelfLocalizer.h:22
Definition: map.h:61
static double sZMax
Definition: SelfLocalizer.h:92
std::string mOdometryFrame
Definition: SelfLocalizer.h:96
static int sMaxBeams
Definition: SelfLocalizer.h:75
double mRangeMax
Definition: SelfLocalizer.h:33
std::string mMapFrame
Definition: SelfLocalizer.h:95
double mPopulationZ
static double sAlpha4
Definition: SelfLocalizer.h:71
static double sZRand
Definition: SelfLocalizer.h:93
ros::Publisher mParticlePublisher
static double sLikelihoodMaxDist
Definition: SelfLocalizer.h:78
int mRangeCount
Definition: SelfLocalizer.h:32
tf::Transform mMapToOdometry
static map_t * sMap
Definition: SelfLocalizer.h:81
tf::TransformListener mTransformListener
bool mPublishParticles
std::string mRobotFrame
Definition: SelfLocalizer.h:97
void(* pf_action_model_fn_t)(void *action_data, struct _pf_sample_set_t *set)
Definition: pf.h:49
pf_sensor_model_fn_t mSensorModelFunction
static double sSigmaHit
Definition: SelfLocalizer.h:84
static Time now()
double mPopulationErr
static tf::StampedTransform mLastPose
static double sLamdaShort
Definition: SelfLocalizer.h:87
pf_t * mParticleFilter
bool mFirstScanReceived


nav2d_localizer
Author(s): Sebastian Kasperski
autogenerated on Tue Nov 7 2017 06:02:33