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_->changed()) {
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)
throw()
1133 dynamic_cast<LocalizationInterface::SetInitialPoseMessage *>(message);
1137 tf::Pose pose = tf::Transform(
1141 const double *covariance = ipm->
covariance();
1142 set_initial_pose(ipm->
frame(), msg_time, pose, covariance);