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-2015 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 
32 #include "amcl_utils.h"
33 #ifdef HAVE_ROS
34 # include "ros_thread.h"
35 #endif
36 
37 #include <baseapp/run.h>
38 #include <core/threading/mutex.h>
39 #include <core/threading/mutex_locker.h>
40 #include <utils/math/angle.h>
41 
42 #include <cstdio>
43 #include <cstdlib>
44 #include <cstring>
45 #include <libgen.h>
46 #include <limits>
47 
48 using namespace fawkes;
49 
50 static double
51 normalize(double z)
52 {
53  return atan2(sin(z), cos(z));
54 }
55 
56 static double
57 angle_diff(double a, double b)
58 {
59  double d1, d2;
60  a = normalize(a);
61  b = normalize(b);
62  d1 = a - b;
63  d2 = 2 * M_PI - fabs(d1);
64  if (d1 > 0)
65  d2 *= -1.0;
66  if (fabs(d1) < fabs(d2))
67  return (d1);
68  else
69  return (d2);
70 }
71 
72 /** @class AmclThread "amcl_thread.h"
73  * Thread to perform Adaptive Monte Carlo Localization.
74  * @author Tim Niemueller
75  */
76 
77 std::vector<std::pair<int, int>> AmclThread::free_space_indices;
78 
79 /** Constructor. */
80 #ifdef HAVE_ROS
82 #else
84 #endif
85 : Thread("AmclThread", Thread::OPMODE_WAITFORWAKEUP),
88  BlackBoardInterfaceListener("AmclThread")
89 {
90  map_ = NULL;
91  conf_mutex_ = new Mutex();
92 #ifdef HAVE_ROS
93  rt_ = ros_thread;
94 #endif
95 }
96 
97 /** Destructor. */
99 {
100  delete conf_mutex_;
101 }
102 
103 void
105 {
106  map_ = NULL;
107 
108  fawkes::amcl::read_map_config(config,
109  cfg_map_file_,
110  cfg_resolution_,
111  cfg_origin_x_,
112  cfg_origin_y_,
113  cfg_origin_theta_,
114  cfg_occupied_thresh_,
115  cfg_free_thresh_);
116 
117  cfg_laser_ifname_ = config->get_string(AMCL_CFG_PREFIX "laser_interface_id");
118  cfg_pose_ifname_ = config->get_string(AMCL_CFG_PREFIX "pose_interface_id");
119 
120  // If set to true, use the latest available odom->base_link transform.
121  // If false, use the transform that matches the laser data timestamp.
122  cfg_use_latest_odom_ = false;
123  try {
124  cfg_use_latest_odom_ = config->get_bool(AMCL_CFG_PREFIX "use_latest_odom");
125  } catch (Exception &e) {
126  } // ignore, use default
127 
128  map_ = fawkes::amcl::read_map(cfg_map_file_.c_str(),
129  cfg_origin_x_,
130  cfg_origin_y_,
131  cfg_resolution_,
132  cfg_occupied_thresh_,
133  cfg_free_thresh_,
134  free_space_indices);
135  map_width_ = map_->size_x;
136  map_height_ = map_->size_y;
137 
138  logger->log_info(name(),
139  "Size: %ux%u (%zu of %u cells free, this are %.1f%%)",
140  map_width_,
141  map_height_,
142  free_space_indices.size(),
143  map_width_ * map_height_,
144  (float)free_space_indices.size() / (float)(map_width_ * map_height_) * 100.);
145 
146  save_pose_last_time.set_clock(clock);
147  save_pose_last_time.stamp();
148 
149  sent_first_transform_ = false;
150  latest_tf_valid_ = false;
151  pf_ = NULL;
152  resample_count_ = 0;
153  odom_ = NULL;
154  laser_ = NULL;
155  // private_nh_="~";
156  initial_pose_hyp_ = NULL;
157  first_map_received_ = false;
158  first_reconfigure_call_ = true;
159 
160  init_pose_[0] = 0.0;
161  init_pose_[1] = 0.0;
162  init_pose_[2] = 0.0;
163  init_cov_[0] = 0.5 * 0.5;
164  init_cov_[1] = 0.5 * 0.5;
165  init_cov_[2] = (M_PI / 12.0) * (M_PI / 12.0);
166 
167  save_pose_period_ = config->get_float(AMCL_CFG_PREFIX "save_pose_period");
168  laser_min_range_ = config->get_float(AMCL_CFG_PREFIX "laser_min_range");
169  laser_max_range_ = config->get_float(AMCL_CFG_PREFIX "laser_max_range");
170  pf_err_ = config->get_float(AMCL_CFG_PREFIX "kld_err");
171  pf_z_ = config->get_float(AMCL_CFG_PREFIX "kld_z");
172  alpha1_ = config->get_float(AMCL_CFG_PREFIX "alpha1");
173  alpha2_ = config->get_float(AMCL_CFG_PREFIX "alpha2");
174  alpha3_ = config->get_float(AMCL_CFG_PREFIX "alpha3");
175  alpha4_ = config->get_float(AMCL_CFG_PREFIX "alpha4");
176  alpha5_ = config->get_float(AMCL_CFG_PREFIX "alpha5");
177  z_hit_ = config->get_float(AMCL_CFG_PREFIX "z_hit");
178  z_short_ = config->get_float(AMCL_CFG_PREFIX "z_short");
179  z_max_ = config->get_float(AMCL_CFG_PREFIX "z_max");
180  z_rand_ = config->get_float(AMCL_CFG_PREFIX "z_rand");
181  sigma_hit_ = config->get_float(AMCL_CFG_PREFIX "sigma_hit");
182  lambda_short_ = config->get_float(AMCL_CFG_PREFIX "lambda_short");
183  laser_likelihood_max_dist_ = config->get_float(AMCL_CFG_PREFIX "laser_likelihood_max_dist");
184  d_thresh_ = config->get_float(AMCL_CFG_PREFIX "d_thresh");
185  a_thresh_ = config->get_float(AMCL_CFG_PREFIX "a_thresh");
186  t_thresh_ = config->get_float(AMCL_CFG_PREFIX "t_thresh");
187  alpha_slow_ = config->get_float(AMCL_CFG_PREFIX "alpha_slow");
188  alpha_fast_ = config->get_float(AMCL_CFG_PREFIX "alpha_fast");
189  angle_increment_ = deg2rad(config->get_float(AMCL_CFG_PREFIX "angle_increment"));
190  try {
191  angle_min_idx_ = config->get_uint(AMCL_CFG_PREFIX "angle_min_idx");
192  if (angle_min_idx_ > 359) {
193  throw Exception("Angle min index out of bounds");
194  }
195  } catch (Exception &e) {
196  angle_min_idx_ = 0;
197  }
198  try {
199  angle_max_idx_ = config->get_uint(AMCL_CFG_PREFIX "angle_max_idx");
200  if (angle_max_idx_ > 359) {
201  throw Exception("Angle max index out of bounds");
202  }
203  } catch (Exception &e) {
204  angle_max_idx_ = 359;
205  }
206  if (angle_max_idx_ > angle_min_idx_) {
207  angle_range_ = (unsigned int)abs((long)angle_max_idx_ - (long)angle_min_idx_);
208  } else {
209  angle_range_ = (360 - angle_min_idx_) + angle_max_idx_;
210  }
211  angle_min_ = deg2rad(angle_min_idx_);
212 
213  max_beams_ = config->get_uint(AMCL_CFG_PREFIX "max_beams");
214  min_particles_ = config->get_uint(AMCL_CFG_PREFIX "min_particles");
215  max_particles_ = config->get_uint(AMCL_CFG_PREFIX "max_particles");
216  resample_interval_ = config->get_uint(AMCL_CFG_PREFIX "resample_interval");
217 
218  odom_frame_id_ = config->get_string("/frames/odom");
219  base_frame_id_ = config->get_string("/frames/base");
220  global_frame_id_ = config->get_string("/frames/fixed");
221 
222  tf_enable_publisher(odom_frame_id_.c_str());
223 
224  std::string tmp_model_type;
225  tmp_model_type = config->get_string(AMCL_CFG_PREFIX "laser_model_type");
226 
227  if (tmp_model_type == "beam")
228  laser_model_type_ = ::amcl::LASER_MODEL_BEAM;
229  else if (tmp_model_type == "likelihood_field")
230  laser_model_type_ = ::amcl::LASER_MODEL_LIKELIHOOD_FIELD;
231  else {
232  logger->log_warn(name(),
233  "Unknown laser model type \"%s\"; "
234  "defaulting to likelihood_field model",
235  tmp_model_type.c_str());
236  laser_model_type_ = ::amcl::LASER_MODEL_LIKELIHOOD_FIELD;
237  }
238 
239  tmp_model_type = config->get_string(AMCL_CFG_PREFIX "odom_model_type");
240  if (tmp_model_type == "diff")
241  odom_model_type_ = ::amcl::ODOM_MODEL_DIFF;
242  else if (tmp_model_type == "omni")
243  odom_model_type_ = ::amcl::ODOM_MODEL_OMNI;
244  else {
245  logger->log_warn(name(),
246  "Unknown odom model type \"%s\"; defaulting to diff model",
247  tmp_model_type.c_str());
248  odom_model_type_ = ::amcl::ODOM_MODEL_DIFF;
249  }
250 
251  try {
252  init_pose_[0] = config->get_float(AMCL_CFG_PREFIX "init_pose_x");
253  } catch (Exception &e) {
254  } // ignored, use default
255 
256  try {
257  init_pose_[1] = config->get_float(AMCL_CFG_PREFIX "init_pose_y");
258  } catch (Exception &e) {
259  } // ignored, use default
260 
261  try {
262  init_pose_[2] = config->get_float(AMCL_CFG_PREFIX "init_pose_a");
263  } catch (Exception &e) {
264  } // ignored, use default
265 
266  cfg_read_init_cov_ = false;
267  try {
268  cfg_read_init_cov_ = config->get_bool(AMCL_CFG_PREFIX "read_init_cov");
269  } catch (Exception &e) {
270  } // ignored, use default
271 
272  if (cfg_read_init_cov_) {
273  try {
274  init_cov_[0] = config->get_float(AMCL_CFG_PREFIX "init_cov_xx");
275  } catch (Exception &e) {
276  } // ignored, use default
277 
278  try {
279  init_cov_[1] = config->get_float(AMCL_CFG_PREFIX "init_cov_yy");
280  } catch (Exception &e) {
281  } // ignored, use default
282 
283  try {
284  init_cov_[2] = config->get_float(AMCL_CFG_PREFIX "init_cov_aa");
285  } catch (Exception &e) {
286  } // ignored, use default
287  } else {
288  logger->log_debug(name(), "Reading initial covariance from config disabled");
289  }
290 
291  transform_tolerance_ = config->get_float(AMCL_CFG_PREFIX "transform_tolerance");
292 
293  if (min_particles_ > max_particles_) {
294  logger->log_warn(name(),
295  "You've set min_particles to be less than max particles, "
296  "this isn't allowed so they'll be set to be equal.");
297  max_particles_ = min_particles_;
298  }
299 
300  //logger->log_info(name(),"calling pf_alloc with %d min and %d max particles",
301  // min_particles_, max_particles_);
302  pf_ = pf_alloc(min_particles_,
303  max_particles_,
304  alpha_slow_,
305  alpha_fast_,
306  (pf_init_model_fn_t)AmclThread::uniform_pose_generator,
307  (void *)map_);
308 
309  pf_init_model(pf_, (pf_init_model_fn_t)AmclThread::uniform_pose_generator, (void *)map_);
310 
311  pf_->pop_err = pf_err_;
312  pf_->pop_z = pf_z_;
313 
314  // Initialize the filter
315 
316  pf_vector_t pf_init_pose_mean = pf_vector_zero();
317  //pf_init_pose_mean.v[0] = last_published_pose.pose.pose.position.x;
318  //pf_init_pose_mean.v[1] = last_published_pose.pose.pose.position.y;
319  //double *q_values = pos3d_if_->rotation();
320  //tf::Quaternion q(q_values[0], q_values[1], q_values[2], q_values[3]);
321  //btScalar unused_pitch, unused_roll, yaw;
322  //btMatrix3x3(q).getRPY(unused_roll, unused_pitch, yaw);
323 
324  pf_init_pose_mean.v[0] = init_pose_[0];
325  pf_init_pose_mean.v[1] = init_pose_[1];
326  pf_init_pose_mean.v[2] = init_pose_[2];
327 
328  pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
329  pf_init_pose_cov.m[0][0] = init_cov_[0]; //last_covariance_[6 * 0 + 0];
330  pf_init_pose_cov.m[1][1] = init_cov_[1]; //last_covariance_[6 * 1 + 1];
331  pf_init_pose_cov.m[2][2] = init_cov_[2]; //last_covariance_[6 * 5 + 5];
332  pf_init(pf_, &pf_init_pose_mean, &pf_init_pose_cov);
333  pf_init_ = false;
334 
335  initial_pose_hyp_ = new amcl_hyp_t();
336  initial_pose_hyp_->pf_pose_mean = pf_init_pose_mean;
337  initial_pose_hyp_->pf_pose_cov = pf_init_pose_cov;
338 
339  // Instantiate the sensor objects
340  // Odometry
341  odom_ = new ::amcl::AMCLOdom();
342 
343  if (odom_model_type_ == ::amcl::ODOM_MODEL_OMNI)
344  odom_->SetModelOmni(alpha1_, alpha2_, alpha3_, alpha4_, alpha5_);
345  else
346  odom_->SetModelDiff(alpha1_, alpha2_, alpha3_, alpha4_);
347 
348  // Laser
349  laser_ = new ::amcl::AMCLLaser(max_beams_, map_);
350 
351  if (laser_model_type_ == ::amcl::LASER_MODEL_BEAM) {
352  laser_->SetModelBeam(z_hit_, z_short_, z_max_, z_rand_, sigma_hit_, lambda_short_, 0.0);
353  } else {
354  logger->log_info(name(),
355  "Initializing likelihood field model; "
356  "this can take some time on large maps...");
357  laser_->SetModelLikelihoodField(z_hit_, z_rand_, sigma_hit_, laser_likelihood_max_dist_);
358  logger->log_info(name(), "Done initializing likelihood field model.");
359  }
360 
361  laser_if_ = blackboard->open_for_reading<Laser360Interface>(cfg_laser_ifname_.c_str());
362  pos3d_if_ = blackboard->open_for_writing<Position3DInterface>(cfg_pose_ifname_.c_str());
363  loc_if_ = blackboard->open_for_writing<LocalizationInterface>("AMCL");
364 
365  bbil_add_message_interface(loc_if_);
367 
368  laser_if_->read();
369  laser_pose_set_ = set_laser_pose();
370 
371  last_move_time_ = new Time(clock);
372  last_move_time_->stamp();
373 
374  cfg_buffer_enable_ = true;
375  try {
376  cfg_buffer_enable_ = config->get_bool(AMCL_CFG_PREFIX "buffering/enable");
377  } catch (Exception &e) {
378  } // ignored, use default
379 
380  cfg_buffer_debug_ = true;
381  try {
382  cfg_buffer_debug_ = config->get_bool(AMCL_CFG_PREFIX "buffering/debug");
383  } catch (Exception &e) {
384  } // ignored, use default
385 
386  laser_buffered_ = false;
387 
388  if (cfg_buffer_enable_) {
389  laser_if_->resize_buffers(1);
390  }
391 
392  pos3d_if_->set_frame(global_frame_id_.c_str());
393  pos3d_if_->write();
394 
395  char * map_file = strdup(cfg_map_file_.c_str());
396  std::string map_name = basename(map_file);
397  free(map_file);
398  std::string::size_type pos;
399  if (((pos = map_name.rfind(".")) != std::string::npos) && (pos > 0)) {
400  map_name = map_name.substr(0, pos - 1);
401  }
402 
403  loc_if_->set_map(map_name.c_str());
404  loc_if_->write();
405 
406 #ifdef HAVE_ROS
407  if (rt_)
408  rt_->publish_map(global_frame_id_, map_);
409 #endif
410 
411  apply_initial_pose();
412 }
413 
414 void
416 {
417  laser_if_->read();
418 
419  if (!laser_pose_set_) {
420  if (set_laser_pose()) {
421  laser_pose_set_ = true;
422  apply_initial_pose();
423  } else {
424  if (fawkes::runtime::uptime() >= tf_listener->get_cache_time()) {
425  logger->log_warn(name(), "Could not determine laser pose, skipping loop");
426  }
427  return;
428  }
429  }
430 
431  //if (! laser_if_->changed() && ! laser_buffered_) {
432  // logger->log_warn(name(), "Laser data unchanged, skipping loop");
433  // return;
434  //}
435 
436  MutexLocker lock(conf_mutex_);
437 
438  // Where was the robot when this scan was taken?
439  tf::Stamped<tf::Pose> odom_pose;
440  pf_vector_t pose;
441 
442  if (laser_if_->changed()) {
443  if (!get_odom_pose(
444  odom_pose, pose.v[0], pose.v[1], pose.v[2], laser_if_->timestamp(), base_frame_id_)) {
445  if (cfg_buffer_debug_) {
446  logger->log_warn(name(),
447  "Couldn't determine robot's pose "
448  "associated with current laser scan");
449  }
450  if (laser_buffered_) {
451  Time buffer_timestamp(laser_if_->buffer_timestamp(0));
452  if (!get_odom_pose(
453  odom_pose, pose.v[0], pose.v[1], pose.v[2], &buffer_timestamp, base_frame_id_)) {
454  fawkes::Time zero_time(0, 0);
455  if (!get_odom_pose(
456  odom_pose, pose.v[0], pose.v[1], pose.v[2], &zero_time, base_frame_id_)) {
457  // could not even use the buffered scan, buffer current one
458  // and try that one next time, always warn, this is bad
459  logger->log_warn(name(),
460  "Couldn't determine robot's pose "
461  "associated with buffered laser scan nor at "
462  "current time, re-buffering");
463  laser_if_->copy_private_to_buffer(0);
464  return;
465  } else {
466  // we got a transform at some time, it is by far not as good
467  // as the correct value, but will at least allow us to go on
468  laser_buffered_ = false;
469  }
470  } else {
471  // yay, that worked, use that one, re-buffer current data
472  if (cfg_buffer_debug_) {
473  logger->log_warn(name(), "Using buffered laser data, re-buffering current");
474  }
475  laser_if_->read_from_buffer(0);
476  laser_if_->copy_shared_to_buffer(0);
477  }
478  } else if (cfg_buffer_enable_) {
479  if (cfg_buffer_debug_) {
480  logger->log_warn(name(), "Buffering current data for next loop");
481  }
482  laser_if_->copy_private_to_buffer(0);
483  laser_buffered_ = true;
484  return;
485  } else {
486  return;
487  }
488  } else {
489  //logger->log_info(name(), "Fresh data is good, using that");
490  laser_buffered_ = false;
491  }
492  } else if (laser_buffered_) {
493  // either data is good to use now or there is no fresh we can buffer
494  laser_buffered_ = false;
495 
496  Time buffer_timestamp(laser_if_->buffer_timestamp(0));
497  if (get_odom_pose(
498  odom_pose, pose.v[0], pose.v[1], pose.v[2], &buffer_timestamp, base_frame_id_)) {
499  // yay, that worked, use that one
500  if (cfg_buffer_debug_) {
501  logger->log_info(name(), "Using buffered laser data (no changed data)");
502  }
503  laser_if_->read_from_buffer(0);
504  } else {
505  if (cfg_buffer_debug_) {
506  logger->log_error(name(),
507  "Couldn't determine robot's pose "
508  "associated with buffered laser scan (2)");
509  }
510  return;
511  }
512  } else {
513  //logger->log_error(name(), "Neither changed nor buffered data, skipping loop");
514  return;
515  }
516 
517  float *laser_distances = laser_if_->distances();
518 
519  pf_vector_t delta = pf_vector_zero();
520 
521  if (pf_init_) {
522  // Compute change in pose
523  //delta = pf_vector_coord_sub(pose, pf_odom_pose_);
524  delta.v[0] = pose.v[0] - pf_odom_pose_.v[0];
525  delta.v[1] = pose.v[1] - pf_odom_pose_.v[1];
526  delta.v[2] = angle_diff(pose.v[2], pf_odom_pose_.v[2]);
527 
528  fawkes::Time now(clock);
529 
530  // See if we should update the filter
531  bool update =
532  fabs(delta.v[0]) > d_thresh_ || fabs(delta.v[1]) > d_thresh_ || fabs(delta.v[2]) > a_thresh_;
533 
534  if (update) {
535  last_move_time_->stamp();
536  /*
537  logger->log_info(name(), "(%f,%f,%f) vs. (%f,%f,%f) diff (%f|%i,%f|%i,%f|%i)",
538  pose.v[0], pose.v[1], pose.v[2],
539  pf_odom_pose_.v[0], pf_odom_pose_.v[1], pf_odom_pose_.v[2],
540  fabs(delta.v[0]), fabs(delta.v[0]) > d_thresh_,
541  fabs(delta.v[1]), fabs(delta.v[1]) > d_thresh_,
542  fabs(delta.v[2]), fabs(delta.v[2]) > a_thresh_);
543  */
544 
545  laser_update_ = true;
546  } else if ((now - last_move_time_) <= t_thresh_) {
547  laser_update_ = true;
548  }
549  }
550 
551  bool force_publication = false;
552  if (!pf_init_) {
553  //logger->log_debug(name(), "! PF init");
554  // Pose at last filter update
555  pf_odom_pose_ = pose;
556 
557  // Filter is now initialized
558  pf_init_ = true;
559 
560  // Should update sensor data
561  laser_update_ = true;
562  force_publication = true;
563 
564  resample_count_ = 0;
565  } else if (pf_init_ && laser_update_) {
566  //logger->log_debug(name(), "PF init && laser update");
567  //printf("pose\n");
568  //pf_vector_fprintf(pose, stdout, "%.3f");
569 
570  ::amcl::AMCLOdomData odata;
571  odata.pose = pose;
572  // HACK
573  // Modify the delta in the action data so the filter gets
574  // updated correctly
575  odata.delta = delta;
576 
577  // Use the action data to update the filter
578  //logger->log_debug(name(), "Updating Odometry");
579  odom_->UpdateAction(pf_, (::amcl::AMCLSensorData *)&odata);
580 
581  // Pose at last filter update
582  //this->pf_odom_pose = pose;
583  }
584 
585  bool resampled = false;
586  // If the robot has moved, update the filter
587  if (laser_update_) {
588  //logger->log_warn(name(), "laser update");
589 
590  ::amcl::AMCLLaserData ldata;
591  ldata.sensor = laser_;
592  ldata.range_count = angle_range_ + 1;
593 
594  // To account for lasers that are mounted upside-down, we determine the
595  // min, max, and increment angles of the laser in the base frame.
596  //
597  // Construct min and max angles of laser, in the base_link frame.
598  Time latest(0, 0);
599  tf::Quaternion q;
600  q.setEulerZYX(angle_min_, 0.0, 0.0);
601  tf::Stamped<tf::Quaternion> min_q(q, latest, laser_if_->frame());
602  q.setEulerZYX(angle_min_ + angle_increment_, 0.0, 0.0);
603  tf::Stamped<tf::Quaternion> inc_q(q, latest, laser_if_->frame());
604  try {
605  tf_listener->transform_quaternion(base_frame_id_, min_q, min_q);
606  tf_listener->transform_quaternion(base_frame_id_, inc_q, inc_q);
607  } catch (Exception &e) {
608  logger->log_warn(name(),
609  "Unable to transform min/max laser angles "
610  "into base frame");
611  logger->log_warn(name(), e);
612  return;
613  }
614 
615  double angle_min = tf::get_yaw(min_q);
616  double angle_increment = tf::get_yaw(inc_q) - angle_min;
617 
618  // wrapping angle to [-pi .. pi]
619  angle_increment = fmod(angle_increment + 5 * M_PI, 2 * M_PI) - M_PI;
620 
621  // Apply range min/max thresholds, if the user supplied them
622  if (laser_max_range_ > 0.0)
623  ldata.range_max = (float)laser_max_range_;
624  else
625  ldata.range_max = std::numeric_limits<float>::max();
626  double range_min;
627  if (laser_min_range_ > 0.0)
628  range_min = (float)laser_min_range_;
629  else
630  range_min = 0.0;
631  // The AMCLLaserData destructor will free this memory
632  ldata.ranges = new double[ldata.range_count][2];
633 
634  const unsigned int maxlen_dist = laser_if_->maxlenof_distances();
635  for (int i = 0; i < ldata.range_count; ++i) {
636  unsigned int idx = (angle_min_idx_ + i) % maxlen_dist;
637  // amcl doesn't (yet) have a concept of min range. So we'll map short
638  // readings to max range.
639  if (laser_distances[idx] <= range_min)
640  ldata.ranges[i][0] = ldata.range_max;
641  else
642  ldata.ranges[i][0] = laser_distances[idx];
643 
644  // Compute bearing
645  ldata.ranges[i][1] = fmod(angle_min_ + (i * angle_increment), 2 * M_PI);
646  }
647 
648  try {
649  laser_->UpdateSensor(pf_, (::amcl::AMCLSensorData *)&ldata);
650  } catch (Exception &e) {
651  logger->log_warn(name(),
652  "Failed to update laser sensor data, "
653  "exception follows");
654  logger->log_warn(name(), e);
655  }
656 
657  laser_update_ = false;
658 
659  pf_odom_pose_ = pose;
660 
661  // Resample the particles
662  if (!(++resample_count_ % resample_interval_)) {
663  //logger->log_info(name(), "Resample!");
664  pf_update_resample(pf_);
665  resampled = true;
666  }
667 
668 #ifdef HAVE_ROS
669  if (rt_)
670  rt_->publish_pose_array(global_frame_id_, (pf_->sets) + pf_->current_set);
671 #endif
672  }
673 
674  if (resampled || force_publication) {
675  // Read out the current hypotheses
676  double max_weight = 0.0;
677  int max_weight_hyp = -1;
678  std::vector<amcl_hyp_t> hyps;
679  hyps.resize(pf_->sets[pf_->current_set].cluster_count);
680  for (int hyp_count = 0; hyp_count < pf_->sets[pf_->current_set].cluster_count; hyp_count++) {
681  double weight;
682  pf_vector_t pose_mean;
683  pf_matrix_t pose_cov;
684  if (!pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean, &pose_cov)) {
685  logger->log_error(name(), "Couldn't get stats on cluster %d", hyp_count);
686  break;
687  }
688 
689  hyps[hyp_count].weight = weight;
690  hyps[hyp_count].pf_pose_mean = pose_mean;
691  hyps[hyp_count].pf_pose_cov = pose_cov;
692 
693  if (hyps[hyp_count].weight > max_weight) {
694  max_weight = hyps[hyp_count].weight;
695  max_weight_hyp = hyp_count;
696  }
697  }
698 
699  if (max_weight > 0.0) {
700  /*
701  logger->log_debug(name(), "Max weight pose: %.3f %.3f %.3f (weight: %f)",
702  hyps[max_weight_hyp].pf_pose_mean.v[0],
703  hyps[max_weight_hyp].pf_pose_mean.v[1],
704  hyps[max_weight_hyp].pf_pose_mean.v[2], max_weight);
705 
706  puts("");
707  pf_matrix_fprintf(hyps[max_weight_hyp].pf_pose_cov, stdout, "%6.3f");
708  puts("");
709  */
710 
711  // Copy in the covariance, converting from 3-D to 6-D
712  pf_sample_set_t *set = pf_->sets + pf_->current_set;
713  for (int i = 0; i < 2; i++) {
714  for (int j = 0; j < 2; j++) {
715  // Report the overall filter covariance, rather than the
716  // covariance for the highest-weight cluster
717  //last_covariance_[6 * i + j] = hyps[max_weight_hyp].pf_pose_cov.m[i][j];
718  last_covariance_[6 * i + j] = set->cov.m[i][j];
719  }
720  }
721 
722  // Report the overall filter covariance, rather than the
723  // covariance for the highest-weight cluster
724  //last_covariance_[6 * 5 + 5] = hyps[max_weight_hyp].pf_pose_cov.m[2][2];
725  last_covariance_[6 * 5 + 5] = set->cov.m[2][2];
726 
727 #ifdef HAVE_ROS
728  if (rt_)
729  rt_->publish_pose(global_frame_id_, hyps[max_weight_hyp], last_covariance_);
730 #endif
731  //last_published_pose = p;
732  /*
733  logger->log_debug(name(), "New pose: %6.3f %6.3f %6.3f",
734  hyps[max_weight_hyp].pf_pose_mean.v[0],
735  hyps[max_weight_hyp].pf_pose_mean.v[1],
736  hyps[max_weight_hyp].pf_pose_mean.v[2]);
737  */
738 
739  // subtracting base to odom from map to base and send map to odom instead
740  tf::Stamped<tf::Pose> odom_to_map;
741 
742  try {
743  tf::Transform tmp_tf(tf::create_quaternion_from_yaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
744  tf::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
745  hyps[max_weight_hyp].pf_pose_mean.v[1],
746  0.0));
747  Time odom_time;
748  if (cfg_use_latest_odom_) {
749  odom_time = Time(0, 0);
750  } else {
751  odom_time = laser_if_->timestamp();
752  }
753  tf::Stamped<tf::Pose> tmp_tf_stamped(tmp_tf.inverse(), odom_time, base_frame_id_);
754  tf_listener->transform_pose(odom_frame_id_, tmp_tf_stamped, odom_to_map);
755  } catch (Exception &e) {
756  logger->log_warn(name(), "Failed to subtract base to odom transform");
757  return;
758  }
759 
760  latest_tf_ = tf::Transform(tf::Quaternion(odom_to_map.getRotation()),
761  tf::Point(odom_to_map.getOrigin()));
762  latest_tf_valid_ = true;
763 
764  // We want to send a transform that is good up until a
765  // tolerance time so that odom can be used
766  Time transform_expiration = (*laser_if_->timestamp() + transform_tolerance_);
767  tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
768  transform_expiration,
769  global_frame_id_,
770  odom_frame_id_);
771  try {
772  tf_publisher->send_transform(tmp_tf_stamped);
773  } catch (Exception &e) {
774  logger->log_error(name(), "Failed to publish transform: %s", e.what_no_backtrace());
775  }
776 
777  // We need to apply the last transform to the latest odom pose to get
778  // the latest map pose to store. We'll take the covariance from
779  // last_published_pose.
780  tf::Pose map_pose = latest_tf_.inverse() * odom_pose;
781  tf::Quaternion map_att = map_pose.getRotation();
782 
783  double trans[3] = {map_pose.getOrigin().x(), map_pose.getOrigin().y(), 0};
784  double rot[4] = {map_att.x(), map_att.y(), map_att.z(), map_att.w()};
785 
786  if (pos3d_if_->visibility_history() >= 0) {
787  pos3d_if_->set_visibility_history(pos3d_if_->visibility_history() + 1);
788  } else {
789  pos3d_if_->set_visibility_history(1);
790  }
791  pos3d_if_->set_translation(trans);
792  pos3d_if_->set_rotation(rot);
793  pos3d_if_->set_covariance(last_covariance_);
794  pos3d_if_->write();
795 
796  sent_first_transform_ = true;
797  } else {
798  logger->log_error(name(), "No pose!");
799  }
800  } else if (latest_tf_valid_) {
801  // Nothing changed, so we'll just republish the last transform, to keep
802  // everybody happy.
803  Time transform_expiration = (*laser_if_->timestamp() + transform_tolerance_);
804  tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
805  transform_expiration,
806  global_frame_id_,
807  odom_frame_id_);
808  try {
809  tf_publisher->send_transform(tmp_tf_stamped);
810  } catch (Exception &e) {
811  logger->log_error(name(), "Failed to publish transform: %s", e.what_no_backtrace());
812  }
813 
814  // We need to apply the last transform to the latest odom pose to get
815  // the latest map pose to store. We'll take the covariance from
816  // last_published_pose.
817  tf::Pose map_pose = latest_tf_.inverse() * odom_pose;
818  tf::Quaternion map_att = map_pose.getRotation();
819 
820  double trans[3] = {map_pose.getOrigin().x(), map_pose.getOrigin().y(), 0};
821  double rot[4] = {map_att.x(), map_att.y(), map_att.z(), map_att.w()};
822 
823  if (pos3d_if_->visibility_history() >= 0) {
824  pos3d_if_->set_visibility_history(pos3d_if_->visibility_history() + 1);
825  } else {
826  pos3d_if_->set_visibility_history(1);
827  }
828  pos3d_if_->set_translation(trans);
829  pos3d_if_->set_rotation(rot);
830  pos3d_if_->write();
831 
832  // Is it time to save our last pose to the config
833  Time now(clock);
834  if ((save_pose_period_ > 0.0) && (now - save_pose_last_time) >= save_pose_period_) {
835  double yaw, pitch, roll;
836  map_pose.getBasis().getEulerYPR(yaw, pitch, roll);
837 
838  //logger->log_debug(name(), "Saving pose (%f,%f,%f) as initial pose to host config",
839  // map_pose.getOrigin().x(), map_pose.getOrigin().y(), yaw);
840 
841  // Make sure we write the config only once by locking/unlocking it
842  config->lock();
843  try {
844  config->set_float(AMCL_CFG_PREFIX "init_pose_x", map_pose.getOrigin().x());
845  config->set_float(AMCL_CFG_PREFIX "init_pose_y", map_pose.getOrigin().y());
846  config->set_float(AMCL_CFG_PREFIX "init_pose_a", yaw);
847  config->set_float(AMCL_CFG_PREFIX "init_cov_xx", last_covariance_[6 * 0 + 0]);
848  config->set_float(AMCL_CFG_PREFIX "init_cov_yy", last_covariance_[6 * 1 + 1]);
849  config->set_float(AMCL_CFG_PREFIX "init_cov_aa", last_covariance_[6 * 5 + 5]);
850  } catch (Exception &e) {
851  logger->log_warn(name(), "Failed to save pose, exception follows, disabling saving");
852  logger->log_warn(name(), e);
853  save_pose_period_ = 0.0;
854  }
855  config->unlock();
856  save_pose_last_time = now;
857  }
858  } else {
859  if (pos3d_if_->visibility_history() <= 0) {
860  pos3d_if_->set_visibility_history(pos3d_if_->visibility_history() - 1);
861  } else {
862  pos3d_if_->set_visibility_history(-1);
863  }
864  pos3d_if_->write();
865  }
866 }
867 
868 void
870 {
871  blackboard->unregister_listener(this);
872  bbil_remove_message_interface(loc_if_);
873 
874  if (map_) {
875  map_free(map_);
876  map_ = NULL;
877  }
878  delete initial_pose_hyp_;
879  initial_pose_hyp_ = NULL;
880 
881  delete last_move_time_;
882  delete odom_;
883  delete laser_;
884 
885  blackboard->close(laser_if_);
886  blackboard->close(pos3d_if_);
887  blackboard->close(loc_if_);
888 }
889 
890 bool
891 AmclThread::get_odom_pose(tf::Stamped<tf::Pose> &odom_pose,
892  double & x,
893  double & y,
894  double & yaw,
895  const fawkes::Time * t,
896  const std::string & f)
897 {
898  // Get the robot's pose
899  tf::Stamped<tf::Pose> ident(tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0, 0, 0)),
900  t,
901  f);
902  try {
903  tf_listener->transform_pose(odom_frame_id_, ident, odom_pose);
904  } catch (Exception &e) {
905  if (cfg_buffer_debug_) {
906  logger->log_warn(name(), "Failed to compute odom pose (%s)", e.what_no_backtrace());
907  }
908  return false;
909  }
910  x = odom_pose.getOrigin().x();
911  y = odom_pose.getOrigin().y();
912  double pitch, roll;
913  odom_pose.getBasis().getEulerZYX(yaw, pitch, roll);
914 
915  //logger->log_info(name(), "Odom pose: (%f, %f, %f)", x, y, yaw);
916 
917  return true;
918 }
919 
920 bool
921 AmclThread::set_laser_pose()
922 {
923  //logger->log_debug(name(), "Transform 1");
924 
925  std::string laser_frame_id = laser_if_->frame();
926  if (laser_frame_id.empty())
927  return false;
928 
929  fawkes::Time now(clock);
930  tf::Stamped<tf::Pose> ident(tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0, 0, 0)),
931  &now,
932  laser_frame_id);
933  tf::Stamped<tf::Pose> laser_pose;
934  try {
935  tf_listener->transform_pose(base_frame_id_, ident, laser_pose);
936  } catch (fawkes::tf::LookupException &e) {
937  //logger->log_error(name(), "Failed to lookup transform from %s to %s.",
938  // laser_frame_id.c_str(), base_frame_id_.c_str());
939  //logger->log_error(name(), e);
940  return false;
941  } catch (fawkes::tf::TransformException &e) {
942  //logger->log_error(name(), "Transform error from %s to %s, exception follows.",
943  // laser_frame_id.c_str(), base_frame_id_.c_str());
944  //logger->log_error(name(), e);
945  return false;
946  } catch (fawkes::Exception &e) {
947  if (fawkes::runtime::uptime() >= tf_listener->get_cache_time()) {
948  logger->log_error(name(),
949  "Generic exception for transform from %s to %s.",
950  laser_frame_id.c_str(),
951  base_frame_id_.c_str());
952  logger->log_error(name(), e);
953  }
954  return false;
955  }
956 
957  /*
958  tf::Stamped<tf::Pose>
959  ident(tf::Transform(tf::Quaternion(0, 0, 0, 1),
960  tf::Vector3(0, 0, 0)), Time(), laser_frame_id);
961  tf::Stamped<tf::Pose> laser_pose;
962 
963  try {
964  tf_listener->transform_pose(base_frame_id_, ident, laser_pose);
965  } catch (tf::TransformException& e) {
966  logger->log_error(name(), "Couldn't transform from %s to %s, "
967  "even though the message notifier is in use",
968  laser_frame_id.c_str(), base_frame_id_.c_str());
969  logger->log_error(name(), e);
970  return;
971  }
972  */
973 
974  pf_vector_t laser_pose_v;
975  laser_pose_v.v[0] = laser_pose.getOrigin().x();
976  laser_pose_v.v[1] = laser_pose.getOrigin().y();
977 
978  // laser mounting angle gets computed later -> set to 0 here!
979  laser_pose_v.v[2] = tf::get_yaw(laser_pose.getRotation());
980  laser_->SetLaserPose(laser_pose_v);
981  logger->log_debug(name(),
982  "Received laser's pose wrt robot: %.3f %.3f %.3f",
983  laser_pose_v.v[0],
984  laser_pose_v.v[1],
985  laser_pose_v.v[2]);
986 
987  return true;
988 }
989 
990 void
991 AmclThread::apply_initial_pose()
992 {
993  if (initial_pose_hyp_ != NULL && map_ != NULL) {
994  logger->log_info(name(),
995  "Applying pose: %.3f %.3f %.3f "
996  "(cov: %.3f %.3f %.3f, %.3f %.3f %.3f, %.3f %.3f %.3f)",
997  initial_pose_hyp_->pf_pose_mean.v[0],
998  initial_pose_hyp_->pf_pose_mean.v[1],
999  initial_pose_hyp_->pf_pose_mean.v[2],
1000  initial_pose_hyp_->pf_pose_cov.m[0][0],
1001  initial_pose_hyp_->pf_pose_cov.m[0][1],
1002  initial_pose_hyp_->pf_pose_cov.m[0][2],
1003  initial_pose_hyp_->pf_pose_cov.m[1][0],
1004  initial_pose_hyp_->pf_pose_cov.m[1][1],
1005  initial_pose_hyp_->pf_pose_cov.m[1][2],
1006  initial_pose_hyp_->pf_pose_cov.m[2][0],
1007  initial_pose_hyp_->pf_pose_cov.m[2][1],
1008  initial_pose_hyp_->pf_pose_cov.m[2][2]);
1009  pf_init(pf_, &initial_pose_hyp_->pf_pose_mean, &initial_pose_hyp_->pf_pose_cov);
1010  pf_init_ = false;
1011  } else {
1012  logger->log_warn(name(), "Called apply initial pose but no pose to apply");
1013  }
1014 }
1015 
1016 pf_vector_t
1017 AmclThread::uniform_pose_generator(void *arg)
1018 {
1019  map_t *map = (map_t *)arg;
1020 #if NEW_UNIFORM_SAMPLING
1021  unsigned int rand_index = drand48() * free_space_indices.size();
1022  std::pair<int, int> free_point = free_space_indices[rand_index];
1023  pf_vector_t p;
1024  p.v[0] = MAP_WXGX(map, free_point.first);
1025  p.v[1] = MAP_WYGY(map, free_point.second);
1026  p.v[2] = drand48() * 2 * M_PI - M_PI;
1027 #else
1028  double min_x, max_x, min_y, max_y;
1029  min_x = (map->size_x * map->scale) / 2.0 - map->origin_x;
1030  max_x = (map->size_x * map->scale) / 2.0 + map->origin_x;
1031  min_y = (map->size_y * map->scale) / 2.0 - map->origin_y;
1032  max_y = (map->size_y * map->scale) / 2.0 + map->origin_y;
1033 
1034  pf_vector_t p;
1035  for (;;) {
1036  p.v[0] = min_x + drand48() * (max_x - min_x);
1037  p.v[1] = min_y + drand48() * (max_y - min_y);
1038  p.v[2] = drand48() * 2 * M_PI - M_PI;
1039  // Check that it's a free cell
1040  int i, j;
1041  i = MAP_GXWX(map, p.v[0]);
1042  j = MAP_GYWY(map, p.v[1]);
1043  if (MAP_VALID(map, i, j) && (map->cells[MAP_INDEX(map, i, j)].occ_state == -1))
1044  break;
1045  }
1046 #endif
1047  return p;
1048 }
1049 
1050 void
1051 AmclThread::set_initial_pose(const std::string & frame_id,
1052  const fawkes::Time &msg_time,
1053  const tf::Pose & pose,
1054  const double * covariance)
1055 {
1056  MutexLocker lock(conf_mutex_);
1057  if (frame_id == "") {
1058  // This should be removed at some point
1059  logger->log_warn(name(),
1060  "Received initial pose with empty frame_id. "
1061  "You should always supply a frame_id.");
1062  } else if (frame_id != global_frame_id_) {
1063  // We only accept initial pose estimates in the global frame, #5148.
1064  logger->log_warn(name(),
1065  "Ignoring initial pose in frame \"%s\"; "
1066  "initial poses must be in the global frame, \"%s\"",
1067  frame_id.c_str(),
1068  global_frame_id_.c_str());
1069  return;
1070  }
1071 
1072  fawkes::Time latest(0, 0);
1073 
1074  // In case the client sent us a pose estimate in the past, integrate the
1075  // intervening odometric change.
1076  tf::StampedTransform tx_odom;
1077  try {
1078  tf_listener->lookup_transform(
1079  base_frame_id_, latest, base_frame_id_, msg_time, global_frame_id_, tx_odom);
1080  } catch (tf::TransformException &e) {
1081  // If we've never sent a transform, then this is normal, because the
1082  // global_frame_id_ frame doesn't exist. We only care about in-time
1083  // transformation for on-the-move pose-setting, so ignoring this
1084  // startup condition doesn't really cost us anything.
1085  if (sent_first_transform_)
1086  logger->log_warn(name(),
1087  "Failed to transform initial pose "
1088  "in time (%s)",
1089  e.what_no_backtrace());
1090  tx_odom.setIdentity();
1091  } catch (Exception &e) {
1092  logger->log_warn(name(), "Failed to transform initial pose in time");
1093  logger->log_warn(name(), e);
1094  }
1095 
1096  tf::Pose pose_new;
1097  pose_new = tx_odom.inverse() * pose;
1098 
1099  // Transform into the global frame
1100 
1101  logger->log_info(name(),
1102  "Setting pose: %.3f %.3f %.3f",
1103  pose_new.getOrigin().x(),
1104  pose_new.getOrigin().y(),
1105  tf::get_yaw(pose_new));
1106  // Re-initialize the filter
1107  pf_vector_t pf_init_pose_mean = pf_vector_zero();
1108  pf_init_pose_mean.v[0] = pose_new.getOrigin().x();
1109  pf_init_pose_mean.v[1] = pose_new.getOrigin().y();
1110  pf_init_pose_mean.v[2] = tf::get_yaw(pose_new);
1111  pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
1112  // Copy in the covariance, converting from 6-D to 3-D
1113  for (int i = 0; i < 2; i++) {
1114  for (int j = 0; j < 2; j++) {
1115  pf_init_pose_cov.m[i][j] = covariance[6 * i + j];
1116  }
1117  }
1118  pf_init_pose_cov.m[2][2] = covariance[6 * 5 + 5];
1119 
1120  delete initial_pose_hyp_;
1121  initial_pose_hyp_ = new amcl_hyp_t();
1122  initial_pose_hyp_->pf_pose_mean = pf_init_pose_mean;
1123  initial_pose_hyp_->pf_pose_cov = pf_init_pose_cov;
1124  apply_initial_pose();
1125 
1126  last_move_time_->stamp();
1127 }
1128 
1129 bool
1130 AmclThread::bb_interface_message_received(Interface *interface, Message *message) throw()
1131 {
1133  dynamic_cast<LocalizationInterface::SetInitialPoseMessage *>(message);
1134  if (ipm) {
1135  fawkes::Time msg_time(ipm->time_enqueued());
1136 
1137  tf::Pose pose = tf::Transform(
1138  tf::Quaternion(ipm->rotation(0), ipm->rotation(1), ipm->rotation(2), ipm->rotation(3)),
1139  tf::Vector3(ipm->translation(0), ipm->translation(1), ipm->translation(2)));
1140 
1141  const double *covariance = ipm->covariance();
1142  set_initial_pose(ipm->frame(), msg_time, pose, covariance);
1143  }
1144  return false;
1145 }
Thread for ROS integration of the Adaptive Monte Carlo Localization.
Definition: ros_thread.h:53
virtual void init()
Initialize the thread.
virtual void finalize()
Finalize the thread.
virtual void loop()
Code to execute in the thread.
AmclThread()
Constructor.
Definition: amcl_thread.cpp:83
virtual ~AmclThread()
Destructor.
Definition: amcl_thread.cpp:98
BlackBoard interface listener.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
@ BBIL_FLAG_MESSAGES
consider message received events
Definition: blackboard.h:89
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:212
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:185
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
@ WAKEUP_HOOK_SENSOR_PROCESS
sensor data processing thread
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual void set_float(const char *path, float f)=0
Set new value in configuration of type float.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
virtual void lock()=0
Lock the config.
virtual void unlock()=0
Unlock the config.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:494
Laser360Interface Fawkes BlackBoard Interface.
void set_frame(const char *new_frame)
Set frame value.
SetInitialPoseMessage Fawkes BlackBoard Interface Message.
LocalizationInterface Fawkes BlackBoard Interface.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:45
const Time * time_enqueued() const
Get time when message was enqueued.
Definition: message.cpp:242
virtual void log_info(const char *component, const char *format,...)
Log informational message.
Definition: multi.cpp:195
virtual void log_warn(const char *component, const char *format,...)
Log warning message.
Definition: multi.cpp:216
virtual void log_debug(const char *component, const char *format,...)
Log debug message.
Definition: multi.cpp:174
virtual void log_error(const char *component, const char *format,...)
Log error message.
Definition: multi.cpp:237
Mutex locking helper.
Definition: mutex_locker.h:34
Mutex mutual exclusion lock.
Definition: mutex.h:33
Position3DInterface Fawkes BlackBoard Interface.
Thread class encapsulation of pthreads.
Definition: thread.h:46
@ OPMODE_WAITFORWAKEUP
operate in wait-for-wakeup mode
Definition: thread.h:58
A class for handling time.
Definition: time.h:93
Thread aspect to access the transform system.
Definition: tf.h:39
@ BOTH_DEFER_PUBLISHER
create transform listener but defer creation of publisher, cf.
Definition: tf.h:50
A frame could not be looked up.
Definition: exceptions.h:43
Transform that contains a timestamp and frame IDs.
Definition: types.h:92
Wrapper class to add time stamp and frame ID to base types.
Definition: types.h:130
Base class for fawkes tf exceptions.
Definition: exceptions.h:31
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36
Pose hypothesis.
Definition: amcl_thread.h:51