SelfLocalizer.cpp
Go to the documentation of this file.
2 
4 #include <math.h>
5 
6 bool isNaN(double a)
7 {
8  //return (a != a);
9  return boost::math::isnan(a);
10 }
11 
12 double angle_diff(double a, double b)
13 {
14  a = atan2(sin(a),cos(a));
15  b = atan2(sin(b),cos(b));
16  double d1 = a - b;
17  double d2 = 2 * M_PI - fabs(d1);
18  if(d1 > 0)
19  d2 *= -1.0;
20  if(fabs(d1) < fabs(d2))
21  return(d1);
22  else
23  return(d2);
24 }
25 
27 {
28  mDeltaX = pNewPose.getOrigin().x() - pLastPose.getOrigin().x();
29  mDeltaY = pNewPose.getOrigin().y() - pLastPose.getOrigin().y();
30  mDeltaTheta = tf::getYaw(pNewPose.getRotation()) - tf::getYaw(pLastPose.getRotation());
31 }
32 
33 LaserData::LaserData(const sensor_msgs::LaserScan::ConstPtr& scan)
34 {
35  mRangeCount = scan->ranges.size();
36  mRanges = new double[mRangeCount][2];
37  mRangeMax = scan->range_max;
38 
39  double angle_min = scan->angle_min;
40  double angle_inc = scan->angle_increment;
41 
42  angle_inc = fmod(angle_inc + 5*M_PI, 2*M_PI) - M_PI;
43 
44  for(int i = 0; i < mRangeCount; i++)
45  {
46  if(scan->ranges[i] <= scan->range_min)
47  mRanges[i][0] = scan->range_max;
48  else
49  mRanges[i][0] = scan->ranges[i];
50 
51  // Compute bearing
52  mRanges[i][1] = angle_min + (i * angle_inc);
53  }
54 }
55 
57 {
58  delete[] mRanges;
59 }
60 
70 
75 
77 
79 
81  : mPublishParticles(publish)
82 {
84 
85  ros::NodeHandle globalNode;
86  globalNode.param("laser_frame", mLaserFrame, std::string("laser"));
87  globalNode.param("robot_frame", mRobotFrame, std::string("robot"));
88  globalNode.param("odometry_frame", mOdometryFrame, std::string("odometry_base"));
89  globalNode.param("map_frame", mMapFrame, std::string("map"));
90 
91  ros::NodeHandle localNode("~/");
92  localNode.param("min_particles", mMinParticles, 500);
93  localNode.param("max_particles", mMaxParticles, 2500);
94  localNode.param("alpha_slow", mAlphaSlow, 0.001);
95  localNode.param("alpha_fast", mAlphaFast, 0.1);
96  localNode.param("min_translation", mMinTranslation, 0.2);
97  localNode.param("min_rotation", mMinRotation, 0.5);
98  localNode.param("pop_err", mPopulationErr, 0.01);
99  localNode.param("pop_z", mPopulationZ, 0.99);
100 
101  localNode.param("laser_sigma_hit", sSigmaHit, 0.2);
102  localNode.param("laser_lambda_short", sLamdaShort, 0.1);
103  localNode.param("laser_z_hit", sZHit, 0.95);
104  localNode.param("laser_z_max", sZMax, 0.05);
105  localNode.param("laser_z_rand", sZRand, 0.05);
106  localNode.param("laser_z_short", sZShort, 0.1);
107  localNode.param("laser_max_beams", sMaxBeams, 30);
108  localNode.param("laser_likelihood_max_dist", sLikelihoodMaxDist, 2.0);
109 
110  localNode.param("odom_alpha_1", sAlpha1, 0.25);
111  localNode.param("odom_alpha_2", sAlpha2, 0.25);
112  localNode.param("odom_alpha_3", sAlpha3, 0.25);
113  localNode.param("odom_alpha_4", sAlpha4, 0.25);
114 
115  localNode.param("laser_model_type", mLaserModelType, 2);
116  switch(mLaserModelType)
117  {
118  case 2:
120  break;
121  case 1:
122  default:
123  mLaserModelType = 1;
125  }
126 
128  {
129  mParticlePublisher = localNode.advertise<geometry_msgs::PoseArray>("particles", 1, true);
130  }
131 
132  mFirstScanReceived = false;
133  mParticleFilter = NULL;
134 
135  // Apply tf_prefix to all used frame-id's
140 
141  // Use squared distance so we don't need sqrt() later
144 }
145 
147 {
148  if(mParticleFilter)
150  if(sMap)
151  map_free(sMap);
152 }
153 
155 {
156  map_t* map = (map_t*)data;
157 
158  double min_x, max_x, min_y, max_y;
159 
160  min_x = map->origin_x - (map->size_x * map->scale / 2.0);
161  max_x = map->origin_x + (map->size_x * map->scale / 2.0);
162  min_y = map->origin_y - (map->size_y * map->scale / 2.0);
163  max_y = map->origin_y + (map->size_y * map->scale / 2.0);
164 
165  pf_vector_t p;
166 
167  while(true)
168  {
169  p.v[0] = min_x + drand48() * (max_x - min_x);
170  p.v[1] = min_y + drand48() * (max_y - min_y);
171  p.v[2] = drand48() * 2 * M_PI - M_PI;
172 
173  // Check that it's a free cell
174  int i,j;
175  i = MAP_GXWX(map, p.v[0]);
176  j = MAP_GYWY(map, p.v[1]);
177  if(MAP_VALID(map,i,j) && (map->cells[MAP_INDEX(map,i,j)].occ_state == -1))
178  break;
179  }
180  return p;
181 }
182 
184 {
185  // Implement sample_motion_odometry (Prob Rob p 136)
186  double delta_rot1, delta_trans, delta_rot2;
187  double delta_rot1_hat, delta_trans_hat, delta_rot2_hat;
188  double delta_rot1_noise, delta_rot2_noise;
189 
190  // Avoid computing a bearing from two poses that are extremely near each
191  // other (happens on in-place rotation).
192  delta_trans = sqrt(data->mDeltaX * data->mDeltaX + data->mDeltaY * data->mDeltaY);
193  if(delta_trans < 0.01)
194  delta_rot1 = 0.0;
195  else
196  delta_rot1 = angle_diff(atan2(data->mDeltaY, data->mDeltaX), tf::getYaw(mLastPose.getRotation()));
197 
198  delta_rot2 = angle_diff(data->mDeltaTheta, delta_rot1);
199 
200  // We want to treat backward and forward motion symmetrically for the
201  // noise model to be applied below. The standard model seems to assume
202  // forward motion.
203  delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)), fabs(angle_diff(delta_rot1,M_PI)));
204  delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)), fabs(angle_diff(delta_rot2,M_PI)));
205 
206  for (int i = 0; i < set->sample_count; i++)
207  {
208  pf_sample_t* sample = set->samples + i;
209 
210  // Sample pose differences
211  delta_rot1_hat = angle_diff(delta_rot1, pf_ran_gaussian(sAlpha1*delta_rot1_noise*delta_rot1_noise + sAlpha2*delta_trans*delta_trans));
212  delta_trans_hat = delta_trans - pf_ran_gaussian(sAlpha3*delta_trans*delta_trans + sAlpha4*delta_rot1_noise*delta_rot1_noise + sAlpha4*delta_rot2_noise*delta_rot2_noise);
213  delta_rot2_hat = angle_diff(delta_rot2, pf_ran_gaussian(sAlpha1*delta_rot2_noise*delta_rot2_noise + sAlpha2*delta_trans*delta_trans));
214 
215  // Apply sampled update to particle pose
216  sample->pose.v[0] += delta_trans_hat * cos(sample->pose.v[2] + delta_rot1_hat);
217  sample->pose.v[1] += delta_trans_hat * sin(sample->pose.v[2] + delta_rot1_hat);
218  sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat;
219  sample->weight = 1.0 / set->sample_count;
220  }
221 
222  return 0.0;
223 }
224 
226 {
227  pf_sample_t *sample;
228  pf_vector_t pose;
229 
230  double mapRange;
231  double obsRange, obsBearing;
232  double z, pz; // What is z/pz ?
233 
234  double totalWeight = 0.0;
235 
236  for (int j = 0; j < set->sample_count; j++)
237  {
238  sample = set->samples + j;
239  pose = sample->pose;
240 
241  pose = pf_vector_coord_add(pose, sLaserPose);
242 // pose = pf_vector_coord_sub(pose, sLaserPose);
243 
244  double p = 1.0;
245  int step = (data->mRangeCount - 1) / (sMaxBeams - 1);
246  for (int i = 0; i < data->mRangeCount; i+=step)
247  {
248  obsRange = data->mRanges[i][0];
249  obsBearing = data->mRanges[i][1];
250  mapRange = map_calc_range(sMap, pose.v[0], pose.v[1], pose.v[2]+obsBearing, data->mRangeMax);
251  pz = 0.0;
252 
253  // Part 1: good, but noisy, hit
254  z = obsRange - mapRange;
255  pz += sZHit * exp(-(z * z) / (2 * sSigmaHit * sSigmaHit));
256 
257  // Part 2: short reading from unexpected obstacle (e.g., a person)
258  if(z < 0)
259  pz += sZShort * sLamdaShort * exp(- sLamdaShort * obsRange);
260 
261  // Part 3: Failure to detect obstacle, reported as max-range
262  if(obsRange == data->mRangeMax)
263  pz += sZMax * 1.0;
264 
265  // Part 4: Random measurements
266  if(obsRange < data->mRangeMax)
267  pz += sZRand * 1.0/data->mRangeMax;
268 
269  // TODO: outlier rejection for short readings
270 
271  assert(pz <= 1.0);
272  assert(pz >= 0.0);
273 
274  p += pz*pz*pz;
275  }
276  sample->weight *= p;
277  totalWeight += sample->weight;
278  }
279  return totalWeight;
280 }
281 
283 {
284  int i, step;
285  double obsRange, obsBearing;
286  double totalWeight = 0.0;
287  pf_sample_t* sample;
288  pf_vector_t pose;
289  pf_vector_t hit;
290 
291  // Compute the sample weights
292  for (int j = 0; j < set->sample_count; j++)
293  {
294  sample = set->samples + j;
295  pose = sample->pose;
296 
297  // Take into account the laser pose relative to the robot
298  pose = pf_vector_coord_add(sLaserPose, pose);
299 
300  // Pre-compute a couple of things
301  double zHitDenom = 2.0 * sSigmaHit * sSigmaHit;
302  double zRandMult = 1.0 / data->mRangeMax;
303  double p = 1.0;
304 
305  step = (data->mRangeCount - 1) / (sMaxBeams - 1);
306  for (i = 0; i < data->mRangeCount; i += step)
307  {
308  obsRange = data->mRanges[i][0];
309  obsBearing = data->mRanges[i][1];
310 
311  // This model ignores max range readings
312  if(obsRange >= data->mRangeMax)
313  continue;
314 
315  double z = 0.0;
316  double pz = 0.0;
317 
318  // Compute the endpoint of the beam
319  hit.v[0] = pose.v[0] + obsRange * cos(pose.v[2] + obsBearing);
320  hit.v[1] = pose.v[1] + obsRange * sin(pose.v[2] + obsBearing);
321 
322  // Convert to map grid coords.
323  int mi = MAP_GXWX(sMap, hit.v[0]);
324  int mj = MAP_GYWY(sMap, hit.v[1]);
325 
326  // Part 1: Get distance from the hit to closest obstacle.
327  // Off-map penalized as max distance
328  if(!MAP_VALID(sMap, mi, mj))
329  z = sMap->max_occ_dist;
330  else
331  z = sMap->cells[MAP_INDEX(sMap,mi,mj)].occ_dist;
332 
333  // Gaussian model
334  // NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma)
335  pz += sZHit * exp(-(z * z) / zHitDenom);
336 
337  // Part 2: random measurements
338  pz += sZRand * zRandMult;
339 
340  // TODO: outlier rejection for short readings
341 
342  if(pz < 0.0 || pz > 1.0)
343  ROS_WARN("Value pz = %.2f, but it should be in range 0..1", pz);
344 
345  // here we have an ad-hoc weighting scheme for combining beam probs
346  // works well, though...
347  p += (pz*pz*pz);
348  }
349 
350  sample->weight *= p;
351  totalWeight += sample->weight;
352  }
353 
354  return(totalWeight);
355 }
356 
358 {
359  // Create the particle filter
363  (void*) sMap);
364 
366  ROS_INFO("Initialized PF with %d samples.", set->sample_count);
369 
370  // Distribute particles uniformly in the free space of the map
372 
373  // Get the laser pose on the robot
374  tf::StampedTransform laserPose;
376 
377  try
378  {
380  }
381  catch(tf::TransformException e)
382  {
383  return false;
384  }
385 
386  sLaserPose.v[0] = laserPose.getOrigin().getX();
387  sLaserPose.v[1] = laserPose.getOrigin().getY();
388  sLaserPose.v[2] = tf::getYaw(laserPose.getRotation());
389 
390  return true;
391 }
392 
393 void SelfLocalizer::process(const sensor_msgs::LaserScan::ConstPtr& scan)
394 {
395  // Ignore all scans unless we got a map
396  if(sMap == NULL) return;
397 
398  // Get the odometry pose from TF. (We use RobotFrame here instead of LaserFrame,
399  // so the motion model of the particle filter can work correctly.)
400  tf::StampedTransform tfPose;
401  try
402  {
403  mTransformListener.lookupTransform(mOdometryFrame, mRobotFrame, scan->header.stamp, tfPose);
404  }
405  catch(tf::TransformException e)
406  {
407  try
408  {
410  }
411  catch(tf::TransformException e)
412  {
413  ROS_WARN("Failed to compute odometry pose, skipping scan (%s)", e.what());
414  return;
415  }
416  }
417 
418  if(!mFirstScanReceived)
419  {
420  // Process the first laser scan after initialization
421  mLastPose = tfPose;
422  mFirstScanReceived = true;
423  mProcessedScans = 0;
424  }
425 
426  // Update motion model with odometry data
427  OdometryData odom(tfPose, mLastPose);
428 
429  double dist = odom.mDeltaX * odom.mDeltaX + odom.mDeltaY * odom.mDeltaY;
430  if(dist < mMinTranslation && fabs(odom.mDeltaTheta) < mMinRotation)
431  return;
432 
433  mProcessedScans++;
435  mLastPose = tfPose;
436 
437  // Update sensor model with laser data
438  LaserData laser(scan);
440 
441  // Do the resampling step
443 
444  // Create the map-to-odometry transform
445  tf::Transform map2robot = getBestPose();
446  tf::Stamped<tf::Pose> odom2map;
447  try
448  {
449  tf::Stamped<tf::Pose> robot2map;
450  robot2map.setData(map2robot.inverse());
451  robot2map.stamp_ = scan->header.stamp;
452  robot2map.frame_id_ = mRobotFrame;
453 
454  mTransformListener.transformPose(mOdometryFrame, robot2map, odom2map);
455  mMapToOdometry = odom2map.inverse();
456  }
458  {
459  ROS_WARN("Failed to subtract base to odom transform");
460  }
461 
462  // Publish the particles for visualization
464 }
465 
466 void SelfLocalizer::convertMap( const nav_msgs::OccupancyGrid& map_msg )
467 {
468  map_t* map = map_alloc();
469  ROS_ASSERT(map);
470 
471  map->size_x = map_msg.info.width;
472  map->size_y = map_msg.info.height;
473  map->scale = map_msg.info.resolution;
474  map->origin_x = map_msg.info.origin.position.x + (map->size_x / 2) * map->scale;
475  map->origin_y = map_msg.info.origin.position.y + (map->size_y / 2) * map->scale;
476 
477  // Convert to pf-internal format
478  map->cells = (map_cell_t*)malloc(sizeof(map_cell_t)*map->size_x*map->size_y);
479  ROS_ASSERT(map->cells);
480  for(int i = 0; i < map->size_x * map->size_y; i++)
481  {
482  if(map_msg.data[i] == 0)
483  map->cells[i].occ_state = -1;
484  else if(map_msg.data[i] == 100)
485  map->cells[i].occ_state = +1;
486  else
487  map->cells[i].occ_state = 0;
488  }
489 
490  if(sMap)
491  map_free(sMap);
492 
493  sMap = map;
494  if(mLaserModelType == 2)
495  {
496  ROS_INFO("Initializing likelihood field model. This can take some time on large maps...");
498  }
499 }
500 
502 {
504  double max = set->cov.m[0][0];
505  if(set->cov.m[1][1] > max) max = set->cov.m[1][1];
506  if(set->cov.m[2][2] > max) max = set->cov.m[2][2];
507  return max;
508 }
509 
511 {
512  pf_vector_t pose;
513  pose.v[0] = 0;
514  pose.v[1] = 0;
515  pose.v[2] = 0;
517 
518  // Wow, is this really the only way to do this !?!
519  double max_weight = 0.0;
520 
521  for(int i = 0; i < set->cluster_count; i++)
522  {
523  double weight;
524  pf_vector_t pose_mean;
525  pf_matrix_t pose_cov;
526  if (!pf_get_cluster_stats(mParticleFilter, i, &weight, &pose_mean, &pose_cov))
527  {
528  ROS_ERROR("Couldn't get stats on cluster %d", i);
529  break;
530  }
531 
532  if(weight > max_weight)
533  {
534  max_weight = weight;
535  pose = pose_mean;
536  }
537  }
538 
539  if(max_weight > 0.0)
540  {
541  ROS_DEBUG("Determined pose at: %.3f %.3f %.3f", pose.v[0], pose.v[1], pose.v[2]);
542  }else
543  {
544  ROS_ERROR("Could not get pose from particle filter!");
545  }
546 // pose = pf_vector_coord_sub(pose, sLaserPose);
547 // return pose;
548 
549  tf::Transform map2robot;
550  map2robot.setOrigin(tf::Vector3(pose.v[0], pose.v[1], 0));
551  map2robot.setRotation(tf::createQuaternionFromYaw(pose.v[2]));
552 
553  return map2robot;
554 }
555 
557 {
558  if(!mPublishParticles) return;
559 
561 
562  geometry_msgs::PoseArray cloud_msg;
563  cloud_msg.header.stamp = ros::Time::now();
564  cloud_msg.header.frame_id = mMapFrame.c_str();
565  cloud_msg.poses.resize(set->sample_count);
566  for(int i = 0; i < set->sample_count; i++)
567  {
568  double x = set->samples[i].pose.v[0];
569  double y = set->samples[i].pose.v[1];
570  double yaw = set->samples[i].pose.v[2];
571  tf::Pose pose(tf::createQuaternionFromYaw(yaw), tf::Vector3(x, y, 0));
572  tf::poseTFToMsg(pose, cloud_msg.poses[i]);
573 
574  // Check content of the message because sometimes NaN values occur and therefore visualization in rviz breaks
575  geometry_msgs::Pose pose_check = cloud_msg.poses.at(i);
576  geometry_msgs::Point pt = pose_check.position;
577  if(isNaN(pt.x))
578  ROS_WARN("NaN occured at pt.x before publishing particle cloud...");
579  if(isNaN(pt.y))
580  ROS_WARN("NaN occured at pt.y before publishing particle cloud...");
581  if(isNaN(pt.z))
582  ROS_WARN("NaN occured at pt.z before publishing particle cloud...");
583  geometry_msgs::Quaternion ori = pose_check.orientation;
584  if(isNaN(ori.x)){
585  ROS_WARN("NaN occured at ori.x before publishing particle cloud, setting it to zero (original x:%f y:%f yaw:%f) ...", set->samples[i].pose.v[0], set->samples[i].pose.v[1], set->samples[i].pose.v[2]);
586  cloud_msg.poses.at(i).orientation.x = 0;
587  }
588  if(isNaN(ori.y)){
589  ROS_WARN("NaN occured at ori.y before publishing particle cloud, setting it to zero (original x:%f y:%f yaw:%f) ...", set->samples[i].pose.v[0], set->samples[i].pose.v[1], set->samples[i].pose.v[2]);
590  cloud_msg.poses.at(i).orientation.y = 0;
591  }
592  if(isNaN(ori.z))
593  ROS_WARN("NaN occured at ori.z before publishing particle cloud ...");
594  if(isNaN(ori.w))
595  ROS_WARN("NaN occured at ori.w before publishing particle cloud ...");
596 
597  }
598  mParticlePublisher.publish(cloud_msg);
599 }
#define MAP_GXWX(map, x)
Definition: map.h:137
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
TFSIMD_FORCE_INLINE void setRotation(const Quaternion &q)
double pop_err
Definition: pf.h:117
void map_update_cspace(map_t *map, double max_occ_dist)
Definition: map_cspace.cpp:120
static double sZHit
Definition: SelfLocalizer.h:90
double mDeltaX
Definition: SelfLocalizer.h:21
pf_action_model_fn_t mActionModelFunction
void process(const sensor_msgs::LaserScan::ConstPtr &scan)
double mMinRotation
double v[3]
Definition: pf_vector.h:40
void publish(const boost::shared_ptr< M > &message) const
static pf_vector_t sLaserPose
Definition: SelfLocalizer.h:72
void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data)
Definition: pf.c:217
void setData(const T &input)
#define MAP_VALID(map, i, j)
Definition: map.h:141
map_cell_t * cells
Definition: map.h:73
static double getYaw(const Quaternion &bt_q)
Definition: map.h:46
pf_t * pf_alloc(int min_samples, int max_samples, double alpha_slow, double alpha_fast, pf_init_model_fn_t random_pose_fn, void *random_pose_data)
Definition: pf.c:47
double(* mRanges)[2]
Definition: SelfLocalizer.h:34
void publishParticleCloud()
std::string mLaserFrame
Definition: SelfLocalizer.h:98
int size_y
Definition: map.h:70
static pf_vector_t distributeParticles(void *map)
#define MAP_INDEX(map, i, j)
Definition: map.h:144
double origin_x
Definition: map.h:64
static double sZShort
Definition: SelfLocalizer.h:91
TFSIMD_FORCE_INLINE const tfScalar & getY() const
double mDeltaTheta
Definition: SelfLocalizer.h:23
double angle_diff(double a, double b)
double mMinTranslation
double scale
Definition: map.h:67
std::string resolve(const std::string &frame_name)
#define MAP_GYWY(map, y)
Definition: map.h:138
TFSIMD_FORCE_INLINE const tfScalar & y() const
#define ROS_WARN(...)
double occ_dist
Definition: map.h:52
double map_calc_range(map_t *map, double ox, double oy, double oa, double max_range)
Definition: map_range.c:38
int size_x
Definition: map.h:70
void setIdentity()
int pf_get_cluster_stats(pf_t *pf, int cluster, double *weight, pf_vector_t *mean, pf_matrix_t *cov)
Definition: pf.c:596
bool waitForTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const
double mDeltaY
Definition: SelfLocalizer.h:22
TFSIMD_FORCE_INLINE const tfScalar & x() const
ROSCPP_DECL void set(const std::string &key, const XmlRpc::XmlRpcValue &v)
double origin_y
Definition: map.h:64
int occ_state
Definition: map.h:49
static double sAlpha3
Definition: SelfLocalizer.h:71
SelfLocalizer(bool publish=true)
double pop_z
Definition: pf.h:117
void pf_update_action(pf_t *pf, pf_action_model_fn_t action_fn, void *action_data)
Definition: pf.c:203
static Quaternion createQuaternionFromYaw(double yaw)
bool isNaN(double a)
Definition: map.h:61
static double sZMax
Definition: SelfLocalizer.h:92
void convertMap(const nav_msgs::OccupancyGrid &map_msg)
int current_set
Definition: pf.h:121
Definition: pf.h:59
std::string mOdometryFrame
Definition: SelfLocalizer.h:96
#define ROS_INFO(...)
static int sMaxBeams
Definition: SelfLocalizer.h:75
bool param(const std::string &param_name, T &param_val, const T &default_val) const
double mRangeMax
Definition: SelfLocalizer.h:33
std::string mMapFrame
Definition: SelfLocalizer.h:95
double mPopulationZ
TFSIMD_FORCE_INLINE const tfScalar & y() const
static double sAlpha4
Definition: SelfLocalizer.h:71
static double sZRand
Definition: SelfLocalizer.h:93
ros::Publisher mParticlePublisher
static double sLikelihoodMaxDist
Definition: SelfLocalizer.h:78
TFSIMD_FORCE_INLINE const tfScalar & x() const
int mRangeCount
Definition: SelfLocalizer.h:32
Transform inverse() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
tf::Transform mMapToOdometry
void pf_init_model(pf_t *pf, pf_init_model_fn_t init_fn, void *init_data)
Definition: pf.c:169
pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b)
Definition: pf_vector.c:106
static map_t * sMap
Definition: SelfLocalizer.h:81
static double sAlpha1
Definition: SelfLocalizer.h:71
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
TFSIMD_FORCE_INLINE const tfScalar & z() const
tf::TransformListener mTransformListener
bool mPublishParticles
std::string mRobotFrame
Definition: SelfLocalizer.h:97
ros::Time stamp_
unsigned int step
pf_vector_t pose
Definition: pf.h:62
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 void poseTFToMsg(const Pose &bt, geometry_msgs::Pose &msg)
std::string frame_id_
void pf_update_resample(pf_t *pf)
Definition: pf.c:269
void map_free(map_t *map)
Definition: map.c:61
void transformPose(const std::string &target_frame, const geometry_msgs::PoseStamped &stamped_in, geometry_msgs::PoseStamped &stamped_out) const
static double calculateLikelihoodFieldModel(LaserData *data, pf_sample_set_t *set)
Quaternion getRotation() const
static double sSigmaHit
Definition: SelfLocalizer.h:84
LaserData(const sensor_msgs::LaserScan::ConstPtr &scan)
double weight
Definition: pf.h:65
TFSIMD_FORCE_INLINE const tfScalar & getX() const
static Time now()
double mPopulationErr
static double calculateMoveModel(OdometryData *data, pf_sample_set_t *set)
static double sAlpha2
Definition: SelfLocalizer.h:71
static tf::StampedTransform mLastPose
map_t * map_alloc(void)
Definition: map.c:38
double pf_ran_gaussian(double sigma)
Definition: pf_pdf.c:132
#define ROS_ASSERT(cond)
pf_sample_set_t sets[2]
Definition: pf.h:122
static double calculateBeamModel(LaserData *data, pf_sample_set_t *set)
void pf_free(pf_t *pf)
Definition: pf.c:113
static double sLamdaShort
Definition: SelfLocalizer.h:87
double max_occ_dist
Definition: map.h:77
#define ROS_ERROR(...)
TFSIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
tf::Transform getBestPose()
double getCovariance()
pf_t * mParticleFilter
bool mFirstScanReceived
#define ROS_DEBUG(...)


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