Fawkes API  Fawkes Development Version
amcl_thread.cpp
1 /***************************************************************************
2  * amcl_thread.cpp - Thread to perform localization
3  *
4  * Created: Wed May 16 16:04:41 2012
5  * Copyright 2012 Tim Niemueller [www.niemueller.de]
6  * 2012 Daniel Ewert
7  * 2012 Kathrin Goffart (Robotino Hackathon 2012)
8  * 2012 Kilian Hinterwaelder (Robotino Hackathon 2012)
9  * 2012 Marcel Prochnau (Robotino Hackathon 2012)
10  * 2012 Jannik Altgen (Robotino Hackathon 2012)
11  ****************************************************************************/
12 
13 /* This program is free software; you can redistribute it and/or modify
14  * it under the terms of the GNU General Public License as published by
15  * the Free Software Foundation; either version 2 of the License, or
16  * (at your option) any later version.
17  *
18  * This program is distributed in the hope that it will be useful,
19  * but WITHOUT ANY WARRANTY; without even the implied warranty of
20  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
21  * GNU Library General Public License for more details.
22  *
23  * Read the full text in the LICENSE.GPL file in the doc directory.
24  */
25 
26 /* Based on amcl_node.cpp from the ROS amcl package
27  * Copyright (c) 2008, Willow Garage, Inc.
28  */
29 
30 #include "amcl_thread.h"
31 #include "amcl_utils.h"
32 
33 #include <utils/math/angle.h>
34 #include <core/threading/mutex.h>
35 #include <core/threading/mutex_locker.h>
36 #include <cstdlib>
37 #include <cstdio>
38 
39 #ifdef HAVE_ROS
40 # include <ros/node_handle.h>
41 # include <geometry_msgs/PoseArray.h>
42 # ifdef USE_MAP_PUB
43 # include <nav_msgs/OccupancyGrid.h>
44 # endif
45 #endif
46 
47 using namespace fawkes;
48 
49 static double normalize(double z) {
50  return atan2(sin(z), cos(z));
51 }
52 
53 static double angle_diff(double a, double b) {
54  double d1, d2;
55  a = normalize(a);
56  b = normalize(b);
57  d1 = a - b;
58  d2 = 2 * M_PI - fabs(d1);
59  if (d1 > 0)
60  d2 *= -1.0;
61  if (fabs(d1) < fabs(d2))
62  return (d1);
63  else
64  return (d2);
65 }
66 
67 /** @class AmclThread "amcl_thread.h"
68  * Thread to perform Adaptive Monte Carlo Localization.
69  * @author Tim Niemueller
70  */
71 
72 std::vector<std::pair<int,int> > AmclThread::free_space_indices;
73 
74 /** Constructor. */
76  : Thread("AmclThread", Thread::OPMODE_WAITFORWAKEUP),
77  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS),
78  TransformAspect(TransformAspect::BOTH, "Pose")
79 {
80  map_ = NULL;
81  conf_mutex_ = new Mutex();
82 }
83 
84 /** Destructor. */
86 {
87  delete conf_mutex_;
88 }
89 
91 {
92  fawkes::amcl::read_map_config(config, cfg_map_file_, cfg_resolution_, cfg_origin_x_,
93  cfg_origin_y_, cfg_origin_theta_, cfg_occupied_thresh_,
94  cfg_free_thresh_);
95 
96  cfg_laser_ifname_ = config->get_string(CFG_PREFIX"laser_interface_id");
97  cfg_pose_ifname_ = config->get_string(CFG_PREFIX"pose_interface_id");
98 
99  map_ = fawkes::amcl::read_map(cfg_map_file_.c_str(),
100  cfg_origin_x_, cfg_origin_y_, cfg_resolution_,
101  cfg_occupied_thresh_, cfg_free_thresh_, free_space_indices);
102  map_width_ = map_->size_x;
103  map_height_ = map_->size_y;
104 
105  logger->log_info(name(), "Size: %ux%u (%zu of %u cells free, this are %.1f%%)",
106  map_width_, map_height_, free_space_indices.size(),
107  map_width_ * map_height_,
108  (float)free_space_indices.size() / (float)(map_width_ * map_height_) * 100.);
109 
110  sent_first_transform_ = false;
111  latest_tf_valid_ = false;
112  pf_ = NULL;
113  resample_count_ = 0;
114  odom_ = NULL;
115  laser_ = NULL;
116  // private_nh_="~";
117  initial_pose_hyp_ = NULL;
118  first_map_received_ = false;
119  first_reconfigure_call_ = true;
120 
121  init_pose_[0] = 0.0;
122  init_pose_[1] = 0.0;
123  init_pose_[2] = 0.0;
124  init_cov_[0] = 0.5 * 0.5;
125  init_cov_[1] = 0.5 * 0.5;
126  init_cov_[2] = (M_PI / 12.0) * (M_PI / 12.0);
127 
128  save_pose_period = config->get_float(CFG_PREFIX"save_pose_rate");
129  laser_min_range_ = config->get_float(CFG_PREFIX"laser_min_range");
130  laser_max_range_ = config->get_float(CFG_PREFIX"laser_max_range");
131  pf_err_ = config->get_float(CFG_PREFIX"kld_err");
132  pf_z_ = config->get_float(CFG_PREFIX"kld_z");
133  alpha1_ = config->get_float(CFG_PREFIX"alpha1");
134  alpha2_ = config->get_float(CFG_PREFIX"alpha2");
135  alpha3_ = config->get_float(CFG_PREFIX"alpha3");
136  alpha4_ = config->get_float(CFG_PREFIX"alpha4");
137  alpha5_ = config->get_float(CFG_PREFIX"alpha5");
138  z_hit_ = config->get_float(CFG_PREFIX"z_hit");
139  z_short_ = config->get_float(CFG_PREFIX"z_short");
140  z_max_ = config->get_float(CFG_PREFIX"z_max");
141  z_rand_ = config->get_float(CFG_PREFIX"z_rand");
142  sigma_hit_ = config->get_float(CFG_PREFIX"sigma_hit");
143  lambda_short_ = config->get_float(CFG_PREFIX"lambda_short");
144  laser_likelihood_max_dist_ =
145  config->get_float(CFG_PREFIX"laser_likelihood_max_dist");
146  d_thresh_ = config->get_float(CFG_PREFIX"d_thresh");
147  a_thresh_ = config->get_float(CFG_PREFIX"a_thresh");
148  t_thresh_ = config->get_float(CFG_PREFIX"t_thresh");
149  alpha_slow_ = config->get_float(CFG_PREFIX"alpha_slow");
150  alpha_fast_ = config->get_float(CFG_PREFIX"alpha_fast");
151  angle_increment_ = deg2rad(config->get_float(CFG_PREFIX"angle_increment"));
152  try {
153  angle_min_idx_ = config->get_uint(CFG_PREFIX"angle_min_idx");
154  if (angle_min_idx_ > 359) {
155  throw Exception("Angle min index out of bounds");
156  }
157  } catch (Exception &e) {
158  angle_min_idx_ = 0;
159  }
160  try {
161  angle_max_idx_ = config->get_uint(CFG_PREFIX"angle_max_idx");
162  if (angle_max_idx_ > 359) {
163  throw Exception("Angle max index out of bounds");
164  }
165  } catch (Exception &e) {
166  angle_max_idx_ = 359;
167  }
168  angle_range_ = (unsigned int)abs(angle_max_idx_ - angle_min_idx_);
169  angle_min_ = deg2rad(angle_min_idx_);
170 
171  max_beams_ = config->get_uint(CFG_PREFIX"max_beams");
172  min_particles_ = config->get_uint(CFG_PREFIX"min_particles");
173  max_particles_ = config->get_uint(CFG_PREFIX"max_particles");
174  resample_interval_ = config->get_uint(CFG_PREFIX"resample_interval");
175 
176  odom_frame_id_ = config->get_string(CFG_PREFIX"odom_frame_id");
177  base_frame_id_ = config->get_string(CFG_PREFIX"base_frame_id");
178  laser_frame_id_ = config->get_string(CFG_PREFIX"laser_frame_id");
179  global_frame_id_ = config->get_string(CFG_PREFIX"global_frame_id");
180 
181  std::string tmp_model_type;
182  tmp_model_type = config->get_string(CFG_PREFIX"laser_model_type");
183 
184  if (tmp_model_type == "beam")
185  laser_model_type_ = ::amcl::LASER_MODEL_BEAM;
186  else if (tmp_model_type == "likelihood_field")
187  laser_model_type_ = ::amcl::LASER_MODEL_LIKELIHOOD_FIELD;
188  else {
189  logger->log_warn(name(),
190  "Unknown laser model type \"%s\"; "
191  "defaulting to likelihood_field model",
192  tmp_model_type.c_str());
193  laser_model_type_ = ::amcl::LASER_MODEL_LIKELIHOOD_FIELD;
194  }
195 
196  tmp_model_type = config->get_string(CFG_PREFIX"odom_model_type");
197  if (tmp_model_type == "diff")
198  odom_model_type_ = ::amcl::ODOM_MODEL_DIFF;
199  else if (tmp_model_type == "omni")
200  odom_model_type_ = ::amcl::ODOM_MODEL_OMNI;
201  else {
202  logger->log_warn(name(),
203  "Unknown odom model type \"%s\"; defaulting to diff model",
204  tmp_model_type.c_str());
205  odom_model_type_ = ::amcl::ODOM_MODEL_DIFF;
206  }
207 
208  try {
209  init_pose_[0] = config->get_float(CFG_PREFIX"init_pose_x");
210  } catch (Exception &e) {} // ignored, use default
211 
212  try {
213  init_pose_[1] = config->get_float(CFG_PREFIX"init_pose_y");
214  } catch (Exception &e) {} // ignored, use default
215 
216  try {
217  init_pose_[2] = config->get_float(CFG_PREFIX"init_pose_a");
218  } catch (Exception &e) {} // ignored, use default
219 
220  try {
221  init_cov_[0] = config->get_float(CFG_PREFIX"init_cov_xx");
222  } catch (Exception &e) {} // ignored, use default
223 
224  try {
225  init_cov_[1] = config->get_float(CFG_PREFIX"init_cov_yy");
226  } catch (Exception &e) {} // ignored, use default
227 
228  try {
229  init_cov_[2] = config->get_float(CFG_PREFIX"init_cov_aa");
230  } catch (Exception &e) {} // ignored, use default
231 
232  transform_tolerance_ = config->get_float(CFG_PREFIX"transform_tolerance");
233 
234  if (min_particles_ > max_particles_) {
235  logger->log_warn(name(),
236  "You've set min_particles to be less than max particles, "
237  "this isn't allowed so they'll be set to be equal.");
238  max_particles_ = min_particles_;
239  }
240 
241  //logger->log_info(name(),"calling pf_alloc with %d min and %d max particles",
242  // min_particles_, max_particles_);
243  pf_ = pf_alloc(min_particles_, max_particles_, alpha_slow_, alpha_fast_,
244  (pf_init_model_fn_t) AmclThread::uniform_pose_generator,
245  (void *) map_);
246 
247  pf_init_model(pf_, (pf_init_model_fn_t)AmclThread::uniform_pose_generator,
248  (void *)map_);
249 
250  pf_->pop_err = pf_err_;
251  pf_->pop_z = pf_z_;
252 
253  // Initialize the filter
254 
255  pf_vector_t pf_init_pose_mean = pf_vector_zero();
256  //pf_init_pose_mean.v[0] = last_published_pose.pose.pose.position.x;
257  //pf_init_pose_mean.v[1] = last_published_pose.pose.pose.position.y;
258  //double *q_values = pos3d_if_->rotation();
259  //tf::Quaternion q(q_values[0], q_values[1], q_values[2], q_values[3]);
260  //btScalar unused_pitch, unused_roll, yaw;
261  //btMatrix3x3(q).getRPY(unused_roll, unused_pitch, yaw);
262 
263  pf_init_pose_mean.v[0] = init_pose_[0];
264  pf_init_pose_mean.v[1] = init_pose_[1];
265  pf_init_pose_mean.v[2] = init_pose_[2];
266 
267  pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
268  pf_init_pose_cov.m[0][0] = init_cov_[0]; //last_covariance_[6 * 0 + 0];
269  pf_init_pose_cov.m[1][1] = init_cov_[1]; //last_covariance_[6 * 1 + 1];
270  pf_init_pose_cov.m[2][2] = init_cov_[2]; //last_covariance_[6 * 5 + 5];
271  pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);
272  pf_init_ = false;
273 
274  initial_pose_hyp_ = new amcl_hyp_t();
275  initial_pose_hyp_->pf_pose_mean = pf_init_pose_mean;
276  initial_pose_hyp_->pf_pose_cov = pf_init_pose_cov;
277 
278  // Instantiate the sensor objects
279  // Odometry
280  odom_ = new ::amcl::AMCLOdom();
281 
282  if (odom_model_type_ == ::amcl::ODOM_MODEL_OMNI)
283  odom_->SetModelOmni(alpha1_, alpha2_, alpha3_, alpha4_, alpha5_);
284  else
285  odom_->SetModelDiff(alpha1_, alpha2_, alpha3_, alpha4_);
286 
287  // Laser
288  laser_ = new ::amcl::AMCLLaser(max_beams_, map_);
289 
290  if (laser_model_type_ == ::amcl::LASER_MODEL_BEAM) {
291  laser_->SetModelBeam(z_hit_, z_short_, z_max_, z_rand_, sigma_hit_,
292  lambda_short_, 0.0);
293  } else {
294  logger->log_info(name(),
295  "Initializing likelihood field model; "
296  "this can take some time on large maps...");
297  laser_->SetModelLikelihoodField(z_hit_, z_rand_, sigma_hit_,
298  laser_likelihood_max_dist_);
299  logger->log_info(name(), "Done initializing likelihood field model.");
300  }
301 
302  laser_pose_set_ = set_laser_pose();
303 
304  last_move_time_ = new Time(clock);
305  last_move_time_->stamp();
306 
307 #ifdef HAVE_ROS
308  pose_pub_ =
309  rosnode->advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose", 2);
310  particlecloud_pub_ =
311  rosnode->advertise<geometry_msgs::PoseArray>("particlecloud", 2);
312  initial_pose_sub_ =
313  rosnode->subscribe("initialpose", 2,
314  &AmclThread::initial_pose_received, this);
315 # ifdef USE_MAP_PUB
316  map_pub_ = rosnode->advertise<nav_msgs::OccupancyGrid>("map", 1, true);
317  publish_map();
318 # endif
319 #endif
320 
321  laser_if_ =
322  blackboard->open_for_reading<Laser360Interface>(cfg_laser_ifname_.c_str());
323  pos3d_if_ =
324  blackboard->open_for_writing<Position3DInterface>(cfg_pose_ifname_.c_str());
325 
326  pos3d_if_->set_frame(global_frame_id_.c_str());
327  pos3d_if_->write();
328 
329  apply_initial_pose();
330 }
331 
332 
333 void
335 {
336  if (!laser_pose_set_) {
337  if (set_laser_pose()) {
338  laser_pose_set_ = true;
339  apply_initial_pose();
340  } else {
341  logger->log_warn(name(), "Could not determine laser pose, skipping loop");
342  return;
343  }
344  }
345 
346  laser_if_->read();
347  if (! laser_if_->changed()) {
348  //logger->log_warn(name(), "Laser data unchanged, skipping loop");
349  return;
350  }
351  float* laser_distances = laser_if_->distances();
352 
353  MutexLocker lock(conf_mutex_);
354 
355  // Where was the robot when this scan was taken?
356  tf::Stamped<tf::Pose> odom_pose;
357  pf_vector_t pose;
358  Time latest(0, 0);
359  // cannot use laser_if_->timestamp() here, since odometry is updated in
360  // last cycle of main loop while laser is newer -> tf extrapolation
361  if (!get_odom_pose(odom_pose, pose.v[0], pose.v[1], pose.v[2],
362  &latest, base_frame_id_))
363  {
364  logger->log_error(name(), "Couldn't determine robot's pose "
365  "associated with laser scan");
366  return;
367  }
368 
369  pf_vector_t delta = pf_vector_zero();
370 
371  if (pf_init_) {
372  // Compute change in pose
373  //delta = pf_vector_coord_sub(pose, pf_odom_pose_);
374  delta.v[0] = pose.v[0] - pf_odom_pose_.v[0];
375  delta.v[1] = pose.v[1] - pf_odom_pose_.v[1];
376  delta.v[2] = angle_diff(pose.v[2], pf_odom_pose_.v[2]);
377 
378  fawkes::Time now(clock);
379 
380  // See if we should update the filter
381  bool update =
382  fabs(delta.v[0]) > d_thresh_ ||
383  fabs(delta.v[1]) > d_thresh_ ||
384  fabs(delta.v[2]) > a_thresh_;
385 
386  if (update) {
387  last_move_time_->stamp();
388  /*
389  logger->log_info(name(), "(%f,%f,%f) vs. (%f,%f,%f) diff (%f|%i,%f|%i,%f|%i)",
390  pose.v[0], pose.v[1], pose.v[2],
391  pf_odom_pose_.v[0], pf_odom_pose_.v[1], pf_odom_pose_.v[2],
392  fabs(delta.v[0]), fabs(delta.v[0]) > d_thresh_,
393  fabs(delta.v[1]), fabs(delta.v[1]) > d_thresh_,
394  fabs(delta.v[2]), fabs(delta.v[2]) > a_thresh_);
395  */
396 
397  laser_update_ = true;
398  } else if ((now - last_move_time_) <= t_thresh_) {
399  laser_update_ = true;
400  }
401  }
402 
403  bool force_publication = false;
404  if (!pf_init_) {
405  //logger->log_debug(name(), "! PF init");
406  // Pose at last filter update
407  pf_odom_pose_ = pose;
408 
409  // Filter is now initialized
410  pf_init_ = true;
411 
412  // Should update sensor data
413  laser_update_ = true;
414  force_publication = true;
415 
416  resample_count_ = 0;
417  } else if (pf_init_ && laser_update_) {
418  //logger->log_debug(name(), "PF init && laser update");
419  //printf("pose\n");
420  //pf_vector_fprintf(pose, stdout, "%.3f");
421 
422  ::amcl::AMCLOdomData odata;
423  odata.pose = pose;
424  // HACK
425  // Modify the delta in the action data so the filter gets
426  // updated correctly
427  odata.delta = delta;
428 
429  // Use the action data to update the filter
430  //logger->log_debug(name(), "Updating Odometry");
431  odom_->UpdateAction(pf_, (::amcl::AMCLSensorData*) &odata);
432 
433  // Pose at last filter update
434  //this->pf_odom_pose = pose;
435  }
436 
437  bool resampled = false;
438  // If the robot has moved, update the filter
439  if (laser_update_) {
440  //logger->log_warn(name(), "laser update");
441 
442  ::amcl::AMCLLaserData ldata;
443  ldata.sensor = laser_;
444  ldata.range_count = angle_range_ + 1;
445 
446  // To account for lasers that are mounted upside-down, we determine the
447  // min, max, and increment angles of the laser in the base frame.
448  //
449  // Construct min and max angles of laser, in the base_link frame.
450  Time latest(0, 0);
451  tf::Quaternion q;
452  q.setEulerZYX(angle_min_, 0.0, 0.0);
453  tf::Stamped<tf::Quaternion> min_q(q, latest, laser_frame_id_);
454  q.setEulerZYX(angle_min_ + angle_increment_, 0.0, 0.0);
455  tf::Stamped<tf::Quaternion> inc_q(q, latest, laser_frame_id_);
456  try
457  {
458  tf_listener->transform_quaternion(base_frame_id_, min_q, min_q);
459  tf_listener->transform_quaternion(base_frame_id_, inc_q, inc_q);
460  }
461  catch(Exception &e)
462  {
463  logger->log_warn(name(), "Unable to transform min/max laser angles "
464  "into base frame");
465  logger->log_warn(name(), e);
466  return;
467  }
468 
469  double angle_min = tf::get_yaw(min_q);
470  double angle_increment = tf::get_yaw(inc_q) - angle_min;
471 
472  // wrapping angle to [-pi .. pi]
473  angle_increment = fmod(angle_increment + 5 * M_PI, 2 * M_PI) - M_PI;
474 
475  // Apply range min/max thresholds, if the user supplied them
476  if (laser_max_range_ > 0.0)
477  ldata.range_max = (float) laser_max_range_;
478  else
479  ldata.range_max = HUGE;
480  double range_min;
481  if (laser_min_range_ > 0.0)
482  range_min = (float) laser_min_range_;
483  else
484  range_min = 0.0;
485  // The AMCLLaserData destructor will free this memory
486  ldata.ranges = new double[ldata.range_count][2];
487 
488  const unsigned int maxlen_dist = laser_if_->maxlenof_distances();
489  for (int i = 0; i < ldata.range_count; ++i) {
490  unsigned int idx = (angle_min_idx_ + i) % maxlen_dist;
491  // amcl doesn't (yet) have a concept of min range. So we'll map short
492  // readings to max range.
493  if (laser_distances[idx] <= range_min)
494  ldata.ranges[i][0] = ldata.range_max;
495  else
496  ldata.ranges[i][0] = laser_distances[idx];
497 
498  // Compute bearing
499  ldata.ranges[i][1] = angle_min + (idx * angle_increment);
500  }
501 
502  try {
503  laser_->UpdateSensor(pf_, (::amcl::AMCLSensorData*) &ldata);
504  } catch (Exception &e) {
505  logger->log_warn(name(), "Failed to update laser sensor data, "
506  "exception follows");
507  logger->log_warn(name(), e);
508  }
509 
510  laser_update_ = false;
511 
512  pf_odom_pose_ = pose;
513 
514  // Resample the particles
515  if (!(++resample_count_ % resample_interval_)) {
516  //logger->log_info(name(), "Resample!");
517  pf_update_resample(pf_);
518  resampled = true;
519  }
520 
521 #ifdef HAVE_ROS
522  pf_sample_set_t* set = (pf_->sets) + pf_->current_set;
523  logger->log_debug(name(), "Num samples: %d", set->sample_count);
524 
525  // Publish the resulting cloud
526  // TODO: set maximum rate for publishing
527  geometry_msgs::PoseArray cloud_msg;
528  cloud_msg.header.stamp = ros::Time::now();
529  cloud_msg.header.frame_id = global_frame_id_;
530  cloud_msg.poses.resize(set->sample_count);
531  for (int i = 0; i < set->sample_count; i++) {
532  tf::Quaternion q(tf::create_quaternion_from_yaw(set->samples[i].pose.v[2]));
533  cloud_msg.poses[i].position.x = set->samples[i].pose.v[0];
534  cloud_msg.poses[i].position.y = set->samples[i].pose.v[1];
535  cloud_msg.poses[i].position.z = 0.;
536  cloud_msg.poses[i].orientation.x = q.x();
537  cloud_msg.poses[i].orientation.y = q.y();
538  cloud_msg.poses[i].orientation.z = q.z();
539  cloud_msg.poses[i].orientation.w = q.w();
540  }
541 
542  particlecloud_pub_.publish(cloud_msg);
543 #endif
544  }
545 
546  if (resampled || force_publication) {
547  // Read out the current hypotheses
548  double max_weight = 0.0;
549  int max_weight_hyp = -1;
550  std::vector<amcl_hyp_t> hyps;
551  hyps.resize(pf_->sets[pf_->current_set].cluster_count);
552  for (int hyp_count = 0;
553  hyp_count < pf_->sets[pf_->current_set].cluster_count;
554  hyp_count++) {
555  double weight;
556  pf_vector_t pose_mean;
557  pf_matrix_t pose_cov;
558  if (!pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean,
559  &pose_cov)) {
560  logger->log_error(name(), "Couldn't get stats on cluster %d",
561  hyp_count);
562  break;
563  }
564 
565  hyps[hyp_count].weight = weight;
566  hyps[hyp_count].pf_pose_mean = pose_mean;
567  hyps[hyp_count].pf_pose_cov = pose_cov;
568 
569  if (hyps[hyp_count].weight > max_weight) {
570  max_weight = hyps[hyp_count].weight;
571  max_weight_hyp = hyp_count;
572  }
573  }
574 
575  if (max_weight > 0.0) {
576  /*
577  logger->log_debug(name(), "Max weight pose: %.3f %.3f %.3f (weight: %f)",
578  hyps[max_weight_hyp].pf_pose_mean.v[0],
579  hyps[max_weight_hyp].pf_pose_mean.v[1],
580  hyps[max_weight_hyp].pf_pose_mean.v[2], max_weight);
581 
582  puts("");
583  pf_matrix_fprintf(hyps[max_weight_hyp].pf_pose_cov, stdout, "%6.3f");
584  puts("");
585  */
586 #ifdef HAVE_ROS
587  geometry_msgs::PoseWithCovarianceStamped p;
588  // Fill in the header
589  p.header.frame_id = global_frame_id_;
590  p.header.stamp = ros::Time();
591  // Copy in the pose
592  p.pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0];
593  p.pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1];
594  tf::Quaternion q(tf::Vector3(0,0,1),
595  hyps[max_weight_hyp].pf_pose_mean.v[2]);
596  p.pose.pose.orientation.x = q.x();
597  p.pose.pose.orientation.y = q.y();
598  p.pose.pose.orientation.z = q.z();
599  p.pose.pose.orientation.w = q.w();
600  // Copy in the covariance, converting from 3-D to 6-D
601  pf_sample_set_t* set = pf_->sets + pf_->current_set;
602  for (int i = 0; i < 2; i++) {
603  for (int j = 0; j < 2; j++) {
604  // Report the overall filter covariance, rather than the
605  // covariance for the highest-weight cluster
606  //p.covariance[6*i+j] = hyps[max_weight_hyp].pf_pose_cov.m[i][j];
607  last_covariance_[6 * i + j] = set->cov.m[i][j];
608  }
609  }
610 
611  // Report the overall filter covariance, rather than the
612  // covariance for the highest-weight cluster
613  //p.covariance[6*5+5] = hyps[max_weight_hyp].pf_pose_cov.m[2][2];
614  last_covariance_[6 * 5 + 5] = set->cov.m[2][2];
615 
616  pose_pub_.publish(p);
617 #endif
618  //last_published_pose = p;
619  /*
620  logger->log_debug(name(), "New pose: %6.3f %6.3f %6.3f",
621  hyps[max_weight_hyp].pf_pose_mean.v[0],
622  hyps[max_weight_hyp].pf_pose_mean.v[1],
623  hyps[max_weight_hyp].pf_pose_mean.v[2]);
624  */
625 
626  // subtracting base to odom from map to base and send map to odom instead
627  tf::Stamped<tf::Pose> odom_to_map;
628 
629  try {
630  tf::Transform
631  tmp_tf(tf::create_quaternion_from_yaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
632  tf::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
633  hyps[max_weight_hyp].pf_pose_mean.v[1], 0.0));
634  Time latest(0, 0);
635  tf::Stamped<tf::Pose> tmp_tf_stamped(tmp_tf.inverse(),
636  latest, base_frame_id_);
637  tf_listener->transform_pose(odom_frame_id_,
638  tmp_tf_stamped, odom_to_map);
639  } catch (Exception &e) {
640  logger->log_warn(name(),
641  "Failed to subtract base to odom transform");
642  return;
643  }
644 
645  latest_tf_ =
646  tf::Transform(tf::Quaternion(odom_to_map.getRotation()),
647  tf::Point(odom_to_map.getOrigin()));
648  latest_tf_valid_ = true;
649 
650  // We want to send a transform that is good up until a
651  // tolerance time so that odom can be used
652  Time transform_expiration =
653  (*laser_if_->timestamp() + transform_tolerance_);
654  tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
655  transform_expiration,
656  global_frame_id_, odom_frame_id_);
657  tf_publisher->send_transform(tmp_tf_stamped);
658 
659 
660  // We need to apply the last transform to the latest odom pose to get
661  // the latest map pose to store. We'll take the covariance from
662  // last_published_pose.
663  tf::Pose map_pose = latest_tf_.inverse() * odom_pose;
664  tf::Quaternion map_att = map_pose.getRotation();
665 
666  double trans[3] = {map_pose.getOrigin().x(), map_pose.getOrigin().y(), 0};
667  double rot[4] = { map_att.x(), map_att.y(), map_att.z(), map_att.w() };
668 
669  if (pos3d_if_->visibility_history() >= 0) {
670  pos3d_if_->set_visibility_history(pos3d_if_->visibility_history() + 1);
671  } else {
672  pos3d_if_->set_visibility_history(1);
673  }
674  pos3d_if_->set_translation(trans);
675  pos3d_if_->set_rotation(rot);
676  pos3d_if_->write();
677 
678  sent_first_transform_ = true;
679  } else {
680  logger->log_error(name(), "No pose!");
681  }
682  } else if (latest_tf_valid_) {
683  // Nothing changed, so we'll just republish the last transform, to keep
684  // everybody happy.
685  Time transform_expiration =
686  (*laser_if_->timestamp() + transform_tolerance_);
687  tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
688  transform_expiration,
689  global_frame_id_, odom_frame_id_);
690  tf_publisher->send_transform(tmp_tf_stamped);
691 
692  // Is it time to save our last pose to the param server
693  Time now(clock);
694  // We need to apply the last transform to the latest odom pose to get
695  // the latest map pose to store. We'll take the covariance from
696  // last_published_pose.
697  tf::Pose map_pose = latest_tf_.inverse() * odom_pose;
698  tf::Quaternion map_att = map_pose.getRotation();
699 
700  double trans[3] = {map_pose.getOrigin().x(), map_pose.getOrigin().y(), 0};
701  double rot[4] = { map_att.x(), map_att.y(), map_att.z(), map_att.w() };
702 
703  if (pos3d_if_->visibility_history() >= 0) {
704  pos3d_if_->set_visibility_history(pos3d_if_->visibility_history() + 1);
705  } else {
706  pos3d_if_->set_visibility_history(1);
707  }
708  pos3d_if_->set_translation(trans);
709  pos3d_if_->set_rotation(rot);
710  pos3d_if_->write();
711 
712  /*
713  if ((save_pose_period > 0.0) &&
714  (now - save_pose_last_time) >= save_pose_period) {
715  double yaw, pitch, roll;
716  map_pose.getBasis().getEulerYPR(yaw, pitch, roll);
717  private_nh_.setParam("initial_pose_x", map_pose.getOrigin().x());
718  private_nh_.setParam("initial_pose_y", map_pose.getOrigin().y());
719  private_nh_.setParam("initial_pose_a", yaw);
720  private_nh_.setParam("initial_cov_xx",
721  last_published_pose.pose.covariance[6 * 0 + 0]);
722  private_nh_.setParam("initial_cov_yy",
723  last_published_pose.pose.covariance[6 * 1 + 1]);
724  private_nh_.setParam("initial_cov_aa",
725  last_published_pose.pose.covariance[6 * 5 + 5]);
726  save_pose_last_time = now;
727  }
728  */
729  } else {
730  if (pos3d_if_->visibility_history() <= 0) {
731  pos3d_if_->set_visibility_history(pos3d_if_->visibility_history() - 1);
732  } else {
733  pos3d_if_->set_visibility_history(-1);
734  }
735  pos3d_if_->write();
736  }
737 }
738 
740 {
741  if (map_) {
742  map_free(map_);
743  map_ = NULL;
744  }
745  delete initial_pose_hyp_;
746  initial_pose_hyp_ = NULL;
747 
748  delete last_move_time_;
749 
750  blackboard->close(laser_if_);
751  blackboard->close(pos3d_if_);
752 
753 #ifdef HAVE_ROS
754  pose_pub_.shutdown();
755  particlecloud_pub_.shutdown();
756  initial_pose_sub_.shutdown();
757  map_pub_.shutdown();
758 #endif
759 }
760 
761 bool
762 AmclThread::get_odom_pose(tf::Stamped<tf::Pose>& odom_pose, double& x,
763  double& y, double& yaw,
764  const fawkes::Time* t, const std::string& f)
765 {
766  // Get the robot's pose
768  ident(tf::Transform(tf::Quaternion(0, 0, 0, 1),
769  tf::Vector3(0, 0, 0)), t, f);
770  try {
771  tf_listener->transform_pose(odom_frame_id_, ident, odom_pose);
772  } catch (Exception &e) {
773  logger->log_warn(name(),
774  "Failed to compute odom pose, skipping scan (%s)", e.what());
775  return false;
776  }
777  x = odom_pose.getOrigin().x();
778  y = odom_pose.getOrigin().y();
779  double pitch, roll;
780  odom_pose.getBasis().getEulerZYX(yaw, pitch, roll);
781 
782  //logger->log_info(name(), "Odom pose: (%f, %f, %f)", x, y, yaw);
783 
784  return true;
785 }
786 
787 
788 #ifdef HAVE_ROS
789 # ifdef USE_MAP_PUB
790 void
791 AmclThread::publish_map()
792 {
793  nav_msgs::OccupancyGrid msg;
794  msg.info.map_load_time = ros::Time::now();
795  msg.header.stamp = ros::Time::now();
796  msg.header.frame_id = "map";
797 
798  msg.info.width = map_width_;
799  msg.info.height = map_height_;
800  msg.info.resolution = cfg_resolution_;
801  msg.info.origin.position.x = 0.; //map_->origin_x;
802  msg.info.origin.position.y = 0.; //map_->origin_y;
803  msg.info.origin.position.z = 0.0;
804  tf::Quaternion q(tf::create_quaternion_from_yaw(0.));
805  msg.info.origin.orientation.x = q.x();
806  msg.info.origin.orientation.y = q.y();
807  msg.info.origin.orientation.z = q.z();
808  msg.info.origin.orientation.w = q.w();
809 
810  // Allocate space to hold the data
811  msg.data.resize(msg.info.width * msg.info.height);
812 
813  for (unsigned int i = 0; i < msg.info.width * msg.info.height; ++i) {
814  if (map_->cells[i].occ_state == +1) {
815  msg.data[i] = +100;
816  } else if (map_->cells[i].occ_state == -1) {
817  msg.data[i] = 0;
818  } else {
819  msg.data[i] = -1;
820  }
821  }
822 
823  map_pub_.publish(msg);
824 }
825 # endif
826 #endif
827 
828 
829 bool
830 AmclThread::set_laser_pose()
831 {
832  //logger->log_debug(name(), "Transform 1");
833  fawkes::Time now(clock);
835  ident(tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0, 0, 0)),
836  &now, laser_frame_id_);
837  tf::Stamped<tf::Pose> laser_pose;
838  try {
839  tf_listener->transform_pose(base_frame_id_, ident, laser_pose);
840  } catch (fawkes::tf::LookupException& e) {
841  //logger->log_error(name(), "Failed to lookup transform from %s to %s.",
842  // laser_frame_id_.c_str(), base_frame_id_.c_str());
843  //logger->log_error(name(), e);
844  return false;
845  } catch (fawkes::tf::TransformException& e) {
846  //logger->log_error(name(), "Transform error from %s to %s, exception follows.",
847  // laser_frame_id_.c_str(), base_frame_id_.c_str());
848  //logger->log_error(name(), e);
849  return false;
850  } catch (fawkes::Exception& e) {
851  logger->log_error(name(), "Generic exception for transform from %s to %s.",
852  laser_frame_id_.c_str(), base_frame_id_.c_str());
853  logger->log_error(name(), e);
854  return false;
855  }
856 
857  /*
858  tf::Stamped<tf::Pose>
859  ident(tf::Transform(tf::Quaternion(0, 0, 0, 1),
860  tf::Vector3(0, 0, 0)), Time(), laser_frame_id_);
861  tf::Stamped<tf::Pose> laser_pose;
862 
863  try {
864  tf_listener->transform_pose(base_frame_id_, ident, laser_pose);
865  } catch (tf::TransformException& e) {
866  logger->log_error(name(), "Couldn't transform from %s to %s, "
867  "even though the message notifier is in use",
868  laser_frame_id_.c_str(), base_frame_id_.c_str());
869  logger->log_error(name(), e);
870  return;
871  }
872  */
873 
874  pf_vector_t laser_pose_v;
875  laser_pose_v.v[0] = laser_pose.getOrigin().x();
876  laser_pose_v.v[1] = laser_pose.getOrigin().y();
877 
878  // laser mounting angle gets computed later -> set to 0 here!
879  laser_pose_v.v[2] = tf::get_yaw(laser_pose.getRotation());
880  laser_->SetLaserPose(laser_pose_v);
881  logger->log_debug(name(),
882  "Received laser's pose wrt robot: %.3f %.3f %.3f",
883  laser_pose_v.v[0], laser_pose_v.v[1], laser_pose_v.v[2]);
884 
885  return true;
886 }
887 
888 void
889 AmclThread::apply_initial_pose()
890 {
891  if (initial_pose_hyp_ != NULL && map_ != NULL) {
892  logger->log_info(name(), "Applying pose: %.3f %.3f %.3f "
893  "(cov: %.3f %.3f %.3f, %.3f %.3f %.3f, %.3f %.3f %.3f)",
894  initial_pose_hyp_->pf_pose_mean.v[0],
895  initial_pose_hyp_->pf_pose_mean.v[1],
896  initial_pose_hyp_->pf_pose_mean.v[2],
897  initial_pose_hyp_->pf_pose_cov.m[0][0],
898  initial_pose_hyp_->pf_pose_cov.m[0][1],
899  initial_pose_hyp_->pf_pose_cov.m[0][2],
900  initial_pose_hyp_->pf_pose_cov.m[1][0],
901  initial_pose_hyp_->pf_pose_cov.m[1][1],
902  initial_pose_hyp_->pf_pose_cov.m[1][2],
903  initial_pose_hyp_->pf_pose_cov.m[2][0],
904  initial_pose_hyp_->pf_pose_cov.m[2][1],
905  initial_pose_hyp_->pf_pose_cov.m[2][2]);
906  pf_init(pf_, initial_pose_hyp_->pf_pose_mean,
907  initial_pose_hyp_->pf_pose_cov);
908  pf_init_ = false;
909  } else {
910  logger->log_warn(name(), "Called apply initial pose but no pose to apply");
911  }
912 }
913 
914 pf_vector_t
915 AmclThread::uniform_pose_generator(void* arg)
916 {
917  map_t* map = (map_t*) arg;
918 #if NEW_UNIFORM_SAMPLING
919  unsigned int rand_index = drand48() * free_space_indices.size();
920  std::pair<int,int> free_point = free_space_indices[rand_index];
921  pf_vector_t p;
922  p.v[0] = MAP_WXGX(map, free_point.first);
923  p.v[1] = MAP_WYGY(map, free_point.second);
924  p.v[2] = drand48() * 2 * M_PI - M_PI;
925 #else
926  double min_x, max_x, min_y, max_y;
927  min_x = (map->size_x * map->scale) / 2.0 - map->origin_x;
928  max_x = (map->size_x * map->scale) / 2.0 + map->origin_x;
929  min_y = (map->size_y * map->scale) / 2.0 - map->origin_y;
930  max_y = (map->size_y * map->scale) / 2.0 + map->origin_y;
931 
932  pf_vector_t p;
933  for (;;) {
934  p.v[0] = min_x + drand48() * (max_x - min_x);
935  p.v[1] = min_y + drand48() * (max_y - min_y);
936  p.v[2] = drand48() * 2 * M_PI - M_PI;
937  // Check that it's a free cell
938  int i, j;
939  i = MAP_GXWX(map, p.v[0]);
940  j = MAP_GYWY(map, p.v[1]);
941  if (MAP_VALID(map,i,j) && (map->cells[MAP_INDEX(map,i,j)].occ_state == -1))
942  break;
943  }
944 #endif
945  return p;
946 }
947 
948 
949 #ifdef HAVE_ROS
950 void
951 AmclThread::initial_pose_received(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
952 {
953  MutexLocker lock(conf_mutex_);
954  if(msg->header.frame_id == "") {
955  // This should be removed at some point
956  logger->log_warn(name(), "Received initial pose with empty frame_id. "
957  "You should always supply a frame_id.");
958  } else if (tf_listener->resolve(msg->header.frame_id) != tf_listener->resolve(global_frame_id_))
959  {
960  // We only accept initial pose estimates in the global frame, #5148.
961  logger->log_warn(name(),"Ignoring initial pose in frame \"%s\"; "
962  "initial poses must be in the global frame, \"%s\"",
963  msg->header.frame_id.c_str(),
964  global_frame_id_.c_str());
965  return;
966  }
967 
968  fawkes::Time latest(0, 0);
969  fawkes::Time msg_time(msg->header.stamp.sec,
970  msg->header.stamp.nsec / 1000);
971 
972  // In case the client sent us a pose estimate in the past, integrate the
973  // intervening odometric change.
974  tf::StampedTransform tx_odom;
975  try {
976  tf_listener->lookup_transform(base_frame_id_, latest,
977  base_frame_id_, msg_time,
978  global_frame_id_, tx_odom);
979  } catch(tf::TransformException &e) {
980  // If we've never sent a transform, then this is normal, because the
981  // global_frame_id_ frame doesn't exist. We only care about in-time
982  // transformation for on-the-move pose-setting, so ignoring this
983  // startup condition doesn't really cost us anything.
984  if(sent_first_transform_)
985  logger->log_warn(name(), "Failed to transform initial pose "
986  "in time (%s)", e.what());
987  tx_odom.setIdentity();
988  } catch (Exception &e) {
989  logger->log_warn(name(), "Failed to transform initial pose in time");
990  logger->log_warn(name(), e);
991  }
992 
993  tf::Pose pose_old, pose_new;
994  pose_old =
995  tf::Transform(tf::Quaternion(msg->pose.pose.orientation.x,
996  msg->pose.pose.orientation.y,
997  msg->pose.pose.orientation.z,
998  msg->pose.pose.orientation.w),
999  tf::Vector3(msg->pose.pose.position.x,
1000  msg->pose.pose.position.y,
1001  msg->pose.pose.position.z));
1002  pose_new = tx_odom.inverse() * pose_old;
1003 
1004  // Transform into the global frame
1005 
1006  logger->log_info(name(), "Setting pose: %.3f %.3f %.3f",
1007  pose_new.getOrigin().x(),
1008  pose_new.getOrigin().y(),
1009  tf::get_yaw(pose_new));
1010  // Re-initialize the filter
1011  pf_vector_t pf_init_pose_mean = pf_vector_zero();
1012  pf_init_pose_mean.v[0] = pose_new.getOrigin().x();
1013  pf_init_pose_mean.v[1] = pose_new.getOrigin().y();
1014  pf_init_pose_mean.v[2] = tf::get_yaw(pose_new);
1015  pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
1016  // Copy in the covariance, converting from 6-D to 3-D
1017  for(int i=0; i<2; i++) {
1018  for(int j=0; j<2; j++) {
1019  pf_init_pose_cov.m[i][j] = msg->pose.covariance[6*i+j];
1020  }
1021  }
1022  pf_init_pose_cov.m[2][2] = msg->pose.covariance[6*5+5];
1023 
1024  delete initial_pose_hyp_;
1025  initial_pose_hyp_ = new amcl_hyp_t();
1026  initial_pose_hyp_->pf_pose_mean = pf_init_pose_mean;
1027  initial_pose_hyp_->pf_pose_cov = pf_init_pose_cov;
1028  apply_initial_pose();
1029 
1030  last_move_time_->stamp();
1031 }
1032 #endif
Laser360Interface Fawkes BlackBoard Interface.
tf::TransformPublisher * tf_publisher
This is the transform publisher which can be used to publish transforms via the blackboard.
Definition: tf.h:56
void set_frame(const char *new_frame)
Set frame value.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Base class for fawkes tf exceptions.
Definition: exceptions.h:34
Fawkes library namespace.
Mutex locking helper.
Definition: mutex_locker.h:33
A class for handling time.
Definition: time.h:91
void set_translation(unsigned int index, const double new_translation)
Set translation value at given index.
Thread class encapsulation of pthreads.
Definition: thread.h:42
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:495
virtual void send_transform(const StampedTransform &transform)
Publish transform.
void set_rotation(unsigned int index, const double new_rotation)
Set rotation value at given index.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
virtual Interface * open_for_writing(const char *interface_type, const char *identifier)=0
Open interface for writing.
Thread aspect to access the transform system.
Definition: tf.h:38
void transform_pose(const std::string &target_frame, const Stamped< Pose > &stamped_in, Stamped< Pose > &stamped_out) const
Transform a stamped pose into the target frame.
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:45
pf_vector_t pf_pose_mean
Mean of pose esimate.
Definition: amcl_thread.h:61
Thread aspect to use blocked timing.
const Time * timestamp() const
Get timestamp of last write.
Definition: interface.cpp:691
virtual void init()
Initialize the thread.
Definition: amcl_thread.cpp:90
Position3DInterface Fawkes BlackBoard Interface.
virtual const char * what() const
Get primary string.
Definition: exception.cpp:661
void set_visibility_history(const int32_t new_visibility_history)
Set visibility_history value.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:472
Pose hypothesis.
Definition: amcl_thread.h:57
pf_matrix_t pf_pose_cov
Covariance of pose estimate.
Definition: amcl_thread.h:63
Transform that contains a timestamp and frame IDs.
Definition: types.h:92
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
const char * name() const
Get name of thread.
Definition: thread.h:95
bool changed() const
Check if data has been changed.
Definition: interface.cpp:748
Wrapper class to add time stamp and frame ID to base types.
Definition: types.h:129
int32_t visibility_history() const
Get visibility_history value.
float * distances() const
Get distances value.
AmclThread()
Constructor.
Definition: amcl_thread.cpp:75
virtual void finalize()
Finalize the thread.
virtual ~AmclThread()
Destructor.
Definition: amcl_thread.cpp:85
std::string resolve(const std::string &frame_name)
Resolve transform name.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
void lookup_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const
Lookup transform.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier)=0
Open interface for reading.
void transform_quaternion(const std::string &target_frame, const Stamped< Quaternion > &stamped_in, Stamped< Quaternion > &stamped_out) const
Transform a stamped Quaternion into the target frame.
Time & stamp()
Set this time to the current time.
Definition: time.cpp:783
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
A frame could not be looked up.
Definition: exceptions.h:46
Mutex mutual exclusion lock.
Definition: mutex.h:32
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:37
tf::TransformListener * tf_listener
This is the transform listener which saves transforms published by other threads in the system...
Definition: tf.h:55
size_t maxlenof_distances() const
Get maximum length of distances value.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:43
virtual void loop()
Code to execute in the thread.
virtual void close(Interface *interface)=0
Close interface.