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