30 #include "amcl_thread.h"
32 #include "amcl_utils.h"
34 # include "ros_thread.h"
37 #include <baseapp/run.h>
38 #include <core/threading/mutex.h>
39 #include <core/threading/mutex_locker.h>
40 #include <utils/math/angle.h>
53 return atan2(sin(z), cos(z));
57 angle_diff(
double a,
double b)
63 d2 = 2 * M_PI - fabs(d1);
66 if (fabs(d1) < fabs(d2))
77 std::vector<std::pair<int, int>> AmclThread::free_space_indices;
91 conf_mutex_ =
new Mutex();
108 fawkes::amcl::read_map_config(config,
114 cfg_occupied_thresh_,
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");
122 cfg_use_latest_odom_ =
false;
124 cfg_use_latest_odom_ = config->
get_bool(AMCL_CFG_PREFIX
"use_latest_odom");
128 map_ = fawkes::amcl::read_map(cfg_map_file_.c_str(),
132 cfg_occupied_thresh_,
135 map_width_ = map_->size_x;
136 map_height_ = map_->size_y;
139 "Size: %ux%u (%zu of %u cells free, this are %.1f%%)",
142 free_space_indices.size(),
143 map_width_ * map_height_,
144 (
float)free_space_indices.size() / (
float)(map_width_ * map_height_) * 100.);
146 save_pose_last_time.set_clock(clock);
147 save_pose_last_time.stamp();
149 sent_first_transform_ =
false;
150 latest_tf_valid_ =
false;
156 initial_pose_hyp_ = NULL;
157 first_map_received_ =
false;
158 first_reconfigure_call_ =
true;
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);
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"));
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");
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");
204 angle_max_idx_ = 359;
206 if (angle_max_idx_ > angle_min_idx_) {
207 angle_range_ = (
unsigned int)abs((
long)angle_max_idx_ - (long)angle_min_idx_);
209 angle_range_ = (360 - angle_min_idx_) + angle_max_idx_;
211 angle_min_ =
deg2rad(angle_min_idx_);
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");
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");
222 tf_enable_publisher(odom_frame_id_.c_str());
224 std::string tmp_model_type;
225 tmp_model_type = config->
get_string(AMCL_CFG_PREFIX
"laser_model_type");
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;
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;
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;
246 "Unknown odom model type \"%s\"; defaulting to diff model",
247 tmp_model_type.c_str());
248 odom_model_type_ = ::amcl::ODOM_MODEL_DIFF;
252 init_pose_[0] = config->
get_float(AMCL_CFG_PREFIX
"init_pose_x");
257 init_pose_[1] = config->
get_float(AMCL_CFG_PREFIX
"init_pose_y");
262 init_pose_[2] = config->
get_float(AMCL_CFG_PREFIX
"init_pose_a");
266 cfg_read_init_cov_ =
false;
268 cfg_read_init_cov_ = config->
get_bool(AMCL_CFG_PREFIX
"read_init_cov");
272 if (cfg_read_init_cov_) {
274 init_cov_[0] = config->
get_float(AMCL_CFG_PREFIX
"init_cov_xx");
279 init_cov_[1] = config->
get_float(AMCL_CFG_PREFIX
"init_cov_yy");
284 init_cov_[2] = config->
get_float(AMCL_CFG_PREFIX
"init_cov_aa");
288 logger->
log_debug(name(),
"Reading initial covariance from config disabled");
291 transform_tolerance_ = config->
get_float(AMCL_CFG_PREFIX
"transform_tolerance");
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_;
302 pf_ = pf_alloc(min_particles_,
306 (pf_init_model_fn_t)AmclThread::uniform_pose_generator,
309 pf_init_model(pf_, (pf_init_model_fn_t)AmclThread::uniform_pose_generator, (
void *)map_);
311 pf_->pop_err = pf_err_;
316 pf_vector_t pf_init_pose_mean = pf_vector_zero();
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];
328 pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
329 pf_init_pose_cov.m[0][0] = init_cov_[0];
330 pf_init_pose_cov.m[1][1] = init_cov_[1];
331 pf_init_pose_cov.m[2][2] = init_cov_[2];
332 pf_init(pf_, &pf_init_pose_mean, &pf_init_pose_cov);
336 initial_pose_hyp_->pf_pose_mean = pf_init_pose_mean;
337 initial_pose_hyp_->pf_pose_cov = pf_init_pose_cov;
341 odom_ = new ::amcl::AMCLOdom();
343 if (odom_model_type_ == ::amcl::ODOM_MODEL_OMNI)
344 odom_->SetModelOmni(alpha1_, alpha2_, alpha3_, alpha4_, alpha5_);
346 odom_->SetModelDiff(alpha1_, alpha2_, alpha3_, alpha4_);
349 laser_ = new ::amcl::AMCLLaser(max_beams_, map_);
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);
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.");
365 bbil_add_message_interface(loc_if_);
369 laser_pose_set_ = set_laser_pose();
371 last_move_time_ =
new Time(clock);
372 last_move_time_->stamp();
374 cfg_buffer_enable_ =
true;
376 cfg_buffer_enable_ = config->
get_bool(AMCL_CFG_PREFIX
"buffering/enable");
380 cfg_buffer_debug_ =
true;
382 cfg_buffer_debug_ = config->
get_bool(AMCL_CFG_PREFIX
"buffering/debug");
386 laser_buffered_ =
false;
388 if (cfg_buffer_enable_) {
389 laser_if_->resize_buffers(1);
392 pos3d_if_->
set_frame(global_frame_id_.c_str());
395 char * map_file = strdup(cfg_map_file_.c_str());
396 std::string map_name = basename(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);
403 loc_if_->set_map(map_name.c_str());
408 rt_->publish_map(global_frame_id_, map_);
411 apply_initial_pose();
419 if (!laser_pose_set_) {
420 if (set_laser_pose()) {
421 laser_pose_set_ =
true;
422 apply_initial_pose();
424 if (fawkes::runtime::uptime() >= tf_listener->get_cache_time()) {
425 logger->
log_warn(name(),
"Could not determine laser pose, skipping loop");
442 if (laser_if_->refreshed()) {
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");
450 if (laser_buffered_) {
451 Time buffer_timestamp(laser_if_->buffer_timestamp(0));
453 odom_pose, pose.v[0], pose.v[1], pose.v[2], &buffer_timestamp, base_frame_id_)) {
456 odom_pose, pose.v[0], pose.v[1], pose.v[2], &zero_time, base_frame_id_)) {
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);
468 laser_buffered_ =
false;
472 if (cfg_buffer_debug_) {
473 logger->
log_warn(name(),
"Using buffered laser data, re-buffering current");
475 laser_if_->read_from_buffer(0);
476 laser_if_->copy_shared_to_buffer(0);
478 }
else if (cfg_buffer_enable_) {
479 if (cfg_buffer_debug_) {
480 logger->
log_warn(name(),
"Buffering current data for next loop");
482 laser_if_->copy_private_to_buffer(0);
483 laser_buffered_ =
true;
490 laser_buffered_ =
false;
492 }
else if (laser_buffered_) {
494 laser_buffered_ =
false;
496 Time buffer_timestamp(laser_if_->buffer_timestamp(0));
498 odom_pose, pose.v[0], pose.v[1], pose.v[2], &buffer_timestamp, base_frame_id_)) {
500 if (cfg_buffer_debug_) {
501 logger->
log_info(name(),
"Using buffered laser data (no changed data)");
503 laser_if_->read_from_buffer(0);
505 if (cfg_buffer_debug_) {
507 "Couldn't determine robot's pose "
508 "associated with buffered laser scan (2)");
517 float *laser_distances = laser_if_->distances();
519 pf_vector_t delta = pf_vector_zero();
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]);
532 fabs(delta.v[0]) > d_thresh_ || fabs(delta.v[1]) > d_thresh_ || fabs(delta.v[2]) > a_thresh_;
535 last_move_time_->stamp();
545 laser_update_ =
true;
546 }
else if ((now - last_move_time_) <= t_thresh_) {
547 laser_update_ =
true;
551 bool force_publication =
false;
555 pf_odom_pose_ = pose;
561 laser_update_ =
true;
562 force_publication =
true;
565 }
else if (pf_init_ && laser_update_) {
570 ::amcl::AMCLOdomData odata;
579 odom_->UpdateAction(pf_, (::amcl::AMCLSensorData *)&odata);
585 bool resampled =
false;
590 ::amcl::AMCLLaserData ldata;
591 ldata.sensor = laser_;
592 ldata.range_count = angle_range_ + 1;
600 q.setEulerZYX(angle_min_, 0.0, 0.0);
602 q.setEulerZYX(angle_min_ + angle_increment_, 0.0, 0.0);
605 tf_listener->transform_quaternion(base_frame_id_, min_q, min_q);
606 tf_listener->transform_quaternion(base_frame_id_, inc_q, inc_q);
609 "Unable to transform min/max laser angles "
615 double angle_min = tf::get_yaw(min_q);
616 double angle_increment = tf::get_yaw(inc_q) - angle_min;
619 angle_increment = fmod(angle_increment + 5 * M_PI, 2 * M_PI) - M_PI;
622 if (laser_max_range_ > 0.0)
623 ldata.range_max = (float)laser_max_range_;
625 ldata.range_max = std::numeric_limits<float>::max();
627 if (laser_min_range_ > 0.0)
628 range_min = (float)laser_min_range_;
632 ldata.ranges =
new double[ldata.range_count][2];
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;
639 if (laser_distances[idx] <= range_min)
640 ldata.ranges[i][0] = ldata.range_max;
642 ldata.ranges[i][0] = laser_distances[idx];
645 ldata.ranges[i][1] = fmod(angle_min_ + (i * angle_increment), 2 * M_PI);
649 laser_->UpdateSensor(pf_, (::amcl::AMCLSensorData *)&ldata);
652 "Failed to update laser sensor data, "
653 "exception follows");
657 laser_update_ =
false;
659 pf_odom_pose_ = pose;
662 if (!(++resample_count_ % resample_interval_)) {
664 pf_update_resample(pf_);
670 rt_->publish_pose_array(global_frame_id_, (pf_->sets) + pf_->current_set);
674 if (resampled || force_publication) {
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++) {
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);
689 hyps[hyp_count].weight = weight;
690 hyps[hyp_count].pf_pose_mean = pose_mean;
691 hyps[hyp_count].pf_pose_cov = pose_cov;
693 if (hyps[hyp_count].weight > max_weight) {
694 max_weight = hyps[hyp_count].weight;
695 max_weight_hyp = hyp_count;
699 if (max_weight > 0.0) {
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++) {
718 last_covariance_[6 * i + j] = set->cov.m[i][j];
725 last_covariance_[6 * 5 + 5] = set->cov.m[2][2];
729 rt_->publish_pose(global_frame_id_, hyps[max_weight_hyp], last_covariance_);
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],
748 if (cfg_use_latest_odom_) {
749 odom_time =
Time(0, 0);
751 odom_time = laser_if_->timestamp();
754 tf_listener->transform_pose(odom_frame_id_, tmp_tf_stamped, odom_to_map);
756 logger->
log_warn(name(),
"Failed to subtract base to odom transform");
760 latest_tf_ = tf::Transform(tf::Quaternion(odom_to_map.getRotation()),
761 tf::Point(odom_to_map.getOrigin()));
762 latest_tf_valid_ =
true;
766 Time transform_expiration = (*laser_if_->timestamp() + transform_tolerance_);
768 transform_expiration,
772 tf_publisher->send_transform(tmp_tf_stamped);
780 tf::Pose map_pose = latest_tf_.inverse() * odom_pose;
781 tf::Quaternion map_att = map_pose.getRotation();
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()};
786 if (pos3d_if_->visibility_history() >= 0) {
787 pos3d_if_->set_visibility_history(pos3d_if_->visibility_history() + 1);
789 pos3d_if_->set_visibility_history(1);
791 pos3d_if_->set_translation(trans);
792 pos3d_if_->set_rotation(rot);
793 pos3d_if_->set_covariance(last_covariance_);
796 sent_first_transform_ =
true;
800 }
else if (latest_tf_valid_) {
803 Time transform_expiration = (*laser_if_->timestamp() + transform_tolerance_);
805 transform_expiration,
809 tf_publisher->send_transform(tmp_tf_stamped);
817 tf::Pose map_pose = latest_tf_.inverse() * odom_pose;
818 tf::Quaternion map_att = map_pose.getRotation();
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()};
823 if (pos3d_if_->visibility_history() >= 0) {
824 pos3d_if_->set_visibility_history(pos3d_if_->visibility_history() + 1);
826 pos3d_if_->set_visibility_history(1);
828 pos3d_if_->set_translation(trans);
829 pos3d_if_->set_rotation(rot);
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);
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]);
851 logger->
log_warn(name(),
"Failed to save pose, exception follows, disabling saving");
853 save_pose_period_ = 0.0;
856 save_pose_last_time = now;
859 if (pos3d_if_->visibility_history() <= 0) {
860 pos3d_if_->set_visibility_history(pos3d_if_->visibility_history() - 1);
862 pos3d_if_->set_visibility_history(-1);
872 bbil_remove_message_interface(loc_if_);
878 delete initial_pose_hyp_;
879 initial_pose_hyp_ = NULL;
881 delete last_move_time_;
885 blackboard->
close(laser_if_);
886 blackboard->
close(pos3d_if_);
887 blackboard->
close(loc_if_);
896 const std::string & f)
903 tf_listener->transform_pose(odom_frame_id_, ident, odom_pose);
905 if (cfg_buffer_debug_) {
910 x = odom_pose.getOrigin().x();
911 y = odom_pose.getOrigin().y();
913 odom_pose.getBasis().getEulerZYX(yaw, pitch, roll);
921 AmclThread::set_laser_pose()
925 std::string laser_frame_id = laser_if_->frame();
926 if (laser_frame_id.empty())
935 tf_listener->transform_pose(base_frame_id_, ident, laser_pose);
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());
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();
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",
991 AmclThread::apply_initial_pose()
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);
1012 logger->
log_warn(name(),
"Called apply initial pose but no pose to apply");
1017 AmclThread::uniform_pose_generator(
void *arg)
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];
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;
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;
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;
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))
1051 AmclThread::set_initial_pose(
const std::string & frame_id,
1053 const tf::Pose & pose,
1054 const double * covariance)
1057 if (frame_id ==
"") {
1060 "Received initial pose with empty frame_id. "
1061 "You should always supply a frame_id.");
1062 }
else if (frame_id != global_frame_id_) {
1065 "Ignoring initial pose in frame \"%s\"; "
1066 "initial poses must be in the global frame, \"%s\"",
1068 global_frame_id_.c_str());
1078 tf_listener->lookup_transform(
1079 base_frame_id_, latest, base_frame_id_, msg_time, global_frame_id_, tx_odom);
1085 if (sent_first_transform_)
1087 "Failed to transform initial pose "
1090 tx_odom.setIdentity();
1092 logger->
log_warn(name(),
"Failed to transform initial pose in time");
1097 pose_new = tx_odom.inverse() * pose;
1102 "Setting pose: %.3f %.3f %.3f",
1103 pose_new.getOrigin().x(),
1104 pose_new.getOrigin().y(),
1105 tf::get_yaw(pose_new));
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();
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];
1118 pf_init_pose_cov.m[2][2] = covariance[6 * 5 + 5];
1120 delete initial_pose_hyp_;
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();
1126 last_move_time_->stamp();
1130 AmclThread::bb_interface_message_received(
Interface *interface,
Message *message) noexcept
1137 tf::Pose pose = tf::Transform(
1141 const double *covariance = ipm->
covariance();
1142 set_initial_pose(ipm->
frame(), msg_time, pose, covariance);
Thread for ROS integration of the Adaptive Monte Carlo Localization.
virtual void init()
Initialize the thread.
virtual void finalize()
Finalize the thread.
virtual void loop()
Code to execute in the thread.
virtual ~AmclThread()
Destructor.
BlackBoard interface listener.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
@ BBIL_FLAG_MESSAGES
consider message received events
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
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.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
@ WAKEUP_HOOK_SENSOR_PROCESS
sensor data processing thread
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual void set_float(const char *path, float f)=0
Set new value in configuration of type float.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
virtual void lock()=0
Lock the config.
virtual void unlock()=0
Unlock the config.
Base class for exceptions in Fawkes.
virtual const char * what_no_backtrace() const noexcept
Get primary string (does not implicitly print the back trace).
Base class for all Fawkes BlackBoard interfaces.
void write()
Write from local copy into BlackBoard memory.
Laser360Interface Fawkes BlackBoard Interface.
void set_frame(const char *new_frame)
Set frame value.
SetInitialPoseMessage Fawkes BlackBoard Interface Message.
double * rotation() const
Get rotation value.
double * covariance() const
Get covariance value.
char * frame() const
Get frame value.
double * translation() const
Get translation value.
LocalizationInterface Fawkes BlackBoard Interface.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
const Time * time_enqueued() const
Get time when message was enqueued.
virtual void log_info(const char *component, const char *format,...)
Log informational message.
virtual void log_warn(const char *component, const char *format,...)
Log warning message.
virtual void log_debug(const char *component, const char *format,...)
Log debug message.
virtual void log_error(const char *component, const char *format,...)
Log error message.
Mutex mutual exclusion lock.
Position3DInterface Fawkes BlackBoard Interface.
Thread class encapsulation of pthreads.
@ OPMODE_WAITFORWAKEUP
operate in wait-for-wakeup mode
A class for handling time.
A frame could not be looked up.
Wrapper class to add time stamp and frame ID to base types.
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.