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
48using namespace fawkes;
49
50static double
51normalize(double z)
52{
53 return atan2(sin(z), cos(z));
54}
55
56static double
57angle_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
77std::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),
86 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS),
87 TransformAspect(TransformAspect::BOTH_DEFER_PUBLISHER),
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
103void
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
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 {
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 {
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_) {
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 {
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());
364
366 blackboard->register_listener(this, BlackBoard::BBIL_FLAG_MESSAGES);
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
414void
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_->refreshed()) {
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_) {
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
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_) {
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) {
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) {
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
868void
870{
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
890bool
891AmclThread::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
920bool
921AmclThread::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()) {
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);
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
990void
991AmclThread::apply_initial_pose()
992{
993 if (initial_pose_hyp_ != NULL && map_ != NULL) {
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
1016pf_vector_t
1017AmclThread::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
1050void
1051AmclThread::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 {
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
1129bool
1130AmclThread::bb_interface_message_received(Interface *interface, Message *message) noexcept
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 * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
BlackBoard interface listener.
void bbil_add_message_interface(Interface *interface)
Add an interface to the message received watch list.
void bbil_remove_message_interface(Interface *interface)
Remove an interface to the message received watch list.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
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.
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
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 noexcept
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
void copy_shared_to_buffer(unsigned int buffer)
Copy data from private memory to buffer.
Definition: interface.cpp:1296
void copy_private_to_buffer(unsigned int buffer)
Copy data from private memory to buffer.
Definition: interface.cpp:1322
void read_from_buffer(unsigned int buffer)
Copy data from buffer to private memory.
Definition: interface.cpp:1338
const Time * timestamp() const
Get timestamp of last write.
Definition: interface.cpp:714
Time buffer_timestamp(unsigned int buffer)
Get time of a buffer.
Definition: interface.cpp:1379
void resize_buffers(unsigned int num_buffers)
Resize buffer array.
Definition: interface.cpp:1261
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:479
bool refreshed() const
Check if data has been refreshed.
Definition: interface.cpp:811
Laser360Interface Fawkes BlackBoard Interface.
float * distances() const
Get distances value.
void set_frame(const char *new_frame)
Set frame value.
char * frame() const
Get frame value.
size_t maxlenof_distances() const
Get maximum length of distances value.
SetInitialPoseMessage Fawkes BlackBoard Interface Message.
LocalizationInterface Fawkes BlackBoard Interface.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:44
const Time * time_enqueued() const
Get time when message was enqueued.
Definition: message.cpp:262
Mutex locking helper.
Definition: mutex_locker.h:34
Mutex mutual exclusion lock.
Definition: mutex.h:33
Position3DInterface Fawkes BlackBoard Interface.
int32_t visibility_history() const
Get visibility_history value.
void set_visibility_history(const int32_t new_visibility_history)
Set visibility_history value.
void set_covariance(unsigned int index, const double new_covariance)
Set covariance value at given index.
void set_rotation(unsigned int index, const double new_rotation)
Set rotation value at given index.
void set_translation(unsigned int index, const double new_translation)
Set translation value at given index.
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
A class for handling time.
Definition: time.h:93
void set_clock(Clock *clock)
Set clock for this instance.
Definition: time.cpp:308
Time & stamp()
Set this time to the current time.
Definition: time.cpp:704
Thread aspect to access the transform system.
Definition: tf.h:39
void tf_enable_publisher(const char *frame_id=0)
Late enabling of publisher.
Definition: tf.cpp:145
tf::TransformPublisher * tf_publisher
This is the transform publisher which can be used to publish transforms via the blackboard.
Definition: tf.h:68
tf::Transformer * tf_listener
This is the transform listener which saves transforms published by other threads in the system.
Definition: tf.h:67
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
virtual void send_transform(const StampedTransform &transform, const bool is_static=false)
Publish transform.
float get_cache_time() const
Get cache time.
void transform_quaternion(const std::string &target_frame, const Stamped< Quaternion > &stamped_in, Stamped< Quaternion > &stamped_out) const
Transform a stamped Quaternion into the target frame.
void transform_pose(const std::string &target_frame, const Stamped< Pose > &stamped_in, Stamped< Pose > &stamped_out) const
Transform a stamped pose into the target frame.
void lookup_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const
Lookup transform.
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
pf_vector_t pf_pose_mean
Mean of pose esimate.
Definition: amcl_thread.h:55
pf_matrix_t pf_pose_cov
Covariance of pose estimate.
Definition: amcl_thread.h:57