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