30 #include "amcl_thread.h" 31 #include "amcl_utils.h" 33 # include "ros_thread.h" 36 #include <utils/math/angle.h> 37 #include <core/threading/mutex.h> 38 #include <core/threading/mutex_locker.h> 39 #include <baseapp/run.h> 48 static double normalize(
double z) {
49 return atan2(sin(z), cos(z));
52 static double angle_diff(
double a,
double b) {
57 d2 = 2 * M_PI - fabs(d1);
60 if (fabs(d1) < fabs(d2))
71 std::vector<std::pair<int,int> > AmclThread::free_space_indices;
85 conf_mutex_ =
new Mutex();
101 fawkes::amcl::read_map_config(config, cfg_map_file_, cfg_resolution_, cfg_origin_x_,
102 cfg_origin_y_, cfg_origin_theta_, cfg_occupied_thresh_,
105 cfg_laser_ifname_ = config->
get_string(AMCL_CFG_PREFIX
"laser_interface_id");
106 cfg_pose_ifname_ = config->
get_string(AMCL_CFG_PREFIX
"pose_interface_id");
108 map_ = fawkes::amcl::read_map(cfg_map_file_.c_str(),
109 cfg_origin_x_, cfg_origin_y_, cfg_resolution_,
110 cfg_occupied_thresh_, cfg_free_thresh_, free_space_indices);
111 map_width_ = map_->size_x;
112 map_height_ = map_->size_y;
114 logger->
log_info(name(),
"Size: %ux%u (%zu of %u cells free, this are %.1f%%)",
115 map_width_, map_height_, free_space_indices.size(),
116 map_width_ * map_height_,
117 (float)free_space_indices.size() / (float)(map_width_ * map_height_) * 100.);
119 save_pose_last_time.set_clock(clock);
120 save_pose_last_time.stamp();
122 sent_first_transform_ =
false;
123 latest_tf_valid_ =
false;
129 initial_pose_hyp_ = NULL;
130 first_map_received_ =
false;
131 first_reconfigure_call_ =
true;
136 init_cov_[0] = 0.5 * 0.5;
137 init_cov_[1] = 0.5 * 0.5;
138 init_cov_[2] = (M_PI / 12.0) * (M_PI / 12.0);
140 save_pose_period_ = config->
get_float(AMCL_CFG_PREFIX
"save_pose_period");
141 laser_min_range_ = config->
get_float(AMCL_CFG_PREFIX
"laser_min_range");
142 laser_max_range_ = config->
get_float(AMCL_CFG_PREFIX
"laser_max_range");
143 pf_err_ = config->
get_float(AMCL_CFG_PREFIX
"kld_err");
144 pf_z_ = config->
get_float(AMCL_CFG_PREFIX
"kld_z");
145 alpha1_ = config->
get_float(AMCL_CFG_PREFIX
"alpha1");
146 alpha2_ = config->
get_float(AMCL_CFG_PREFIX
"alpha2");
147 alpha3_ = config->
get_float(AMCL_CFG_PREFIX
"alpha3");
148 alpha4_ = config->
get_float(AMCL_CFG_PREFIX
"alpha4");
149 alpha5_ = config->
get_float(AMCL_CFG_PREFIX
"alpha5");
150 z_hit_ = config->
get_float(AMCL_CFG_PREFIX
"z_hit");
151 z_short_ = config->
get_float(AMCL_CFG_PREFIX
"z_short");
152 z_max_ = config->
get_float(AMCL_CFG_PREFIX
"z_max");
153 z_rand_ = config->
get_float(AMCL_CFG_PREFIX
"z_rand");
154 sigma_hit_ = config->
get_float(AMCL_CFG_PREFIX
"sigma_hit");
155 lambda_short_ = config->
get_float(AMCL_CFG_PREFIX
"lambda_short");
156 laser_likelihood_max_dist_ =
157 config->
get_float(AMCL_CFG_PREFIX
"laser_likelihood_max_dist");
158 d_thresh_ = config->
get_float(AMCL_CFG_PREFIX
"d_thresh");
159 a_thresh_ = config->
get_float(AMCL_CFG_PREFIX
"a_thresh");
160 t_thresh_ = config->
get_float(AMCL_CFG_PREFIX
"t_thresh");
161 alpha_slow_ = config->
get_float(AMCL_CFG_PREFIX
"alpha_slow");
162 alpha_fast_ = config->
get_float(AMCL_CFG_PREFIX
"alpha_fast");
163 angle_increment_ =
deg2rad(config->
get_float(AMCL_CFG_PREFIX
"angle_increment"));
165 angle_min_idx_ = config->
get_uint(AMCL_CFG_PREFIX
"angle_min_idx");
166 if (angle_min_idx_ > 359) {
167 throw Exception(
"Angle min index out of bounds");
173 angle_max_idx_ = config->
get_uint(AMCL_CFG_PREFIX
"angle_max_idx");
174 if (angle_max_idx_ > 359) {
175 throw Exception(
"Angle max index out of bounds");
178 angle_max_idx_ = 359;
180 if (angle_max_idx_ > angle_min_idx_) {
181 angle_range_ = (
unsigned int)abs((
long)angle_max_idx_ - (long)angle_min_idx_);
183 angle_range_ = (360 - angle_min_idx_) + angle_max_idx_;
185 angle_min_ =
deg2rad(angle_min_idx_);
187 max_beams_ = config->
get_uint(AMCL_CFG_PREFIX
"max_beams");
188 min_particles_ = config->
get_uint(AMCL_CFG_PREFIX
"min_particles");
189 max_particles_ = config->
get_uint(AMCL_CFG_PREFIX
"max_particles");
190 resample_interval_ = config->
get_uint(AMCL_CFG_PREFIX
"resample_interval");
192 odom_frame_id_ = config->
get_string(
"/frames/odom");
193 base_frame_id_ = config->
get_string(
"/frames/base");
194 global_frame_id_ = config->
get_string(
"/frames/fixed");
196 tf_enable_publisher(odom_frame_id_.c_str());
198 std::string tmp_model_type;
199 tmp_model_type = config->
get_string(AMCL_CFG_PREFIX
"laser_model_type");
201 if (tmp_model_type ==
"beam")
202 laser_model_type_ = ::amcl::LASER_MODEL_BEAM;
203 else if (tmp_model_type ==
"likelihood_field")
204 laser_model_type_ = ::amcl::LASER_MODEL_LIKELIHOOD_FIELD;
207 "Unknown laser model type \"%s\"; " 208 "defaulting to likelihood_field model",
209 tmp_model_type.c_str());
210 laser_model_type_ = ::amcl::LASER_MODEL_LIKELIHOOD_FIELD;
213 tmp_model_type = config->
get_string(AMCL_CFG_PREFIX
"odom_model_type");
214 if (tmp_model_type ==
"diff")
215 odom_model_type_ = ::amcl::ODOM_MODEL_DIFF;
216 else if (tmp_model_type ==
"omni")
217 odom_model_type_ = ::amcl::ODOM_MODEL_OMNI;
220 "Unknown odom model type \"%s\"; defaulting to diff model",
221 tmp_model_type.c_str());
222 odom_model_type_ = ::amcl::ODOM_MODEL_DIFF;
226 init_pose_[0] = config->
get_float(AMCL_CFG_PREFIX
"init_pose_x");
230 init_pose_[1] = config->
get_float(AMCL_CFG_PREFIX
"init_pose_y");
234 init_pose_[2] = config->
get_float(AMCL_CFG_PREFIX
"init_pose_a");
237 cfg_read_init_cov_ =
false;
239 cfg_read_init_cov_ = config->
get_bool(AMCL_CFG_PREFIX
"read_init_cov");
242 if (cfg_read_init_cov_) {
244 init_cov_[0] = config->
get_float(AMCL_CFG_PREFIX
"init_cov_xx");
248 init_cov_[1] = config->
get_float(AMCL_CFG_PREFIX
"init_cov_yy");
252 init_cov_[2] = config->
get_float(AMCL_CFG_PREFIX
"init_cov_aa");
255 logger->
log_debug(name(),
"Reading initial covariance from config disabled");
258 transform_tolerance_ = config->
get_float(AMCL_CFG_PREFIX
"transform_tolerance");
260 if (min_particles_ > max_particles_) {
262 "You've set min_particles to be less than max particles, " 263 "this isn't allowed so they'll be set to be equal.");
264 max_particles_ = min_particles_;
269 pf_ = pf_alloc(min_particles_, max_particles_, alpha_slow_, alpha_fast_,
270 (pf_init_model_fn_t) AmclThread::uniform_pose_generator,
273 pf_init_model(pf_, (pf_init_model_fn_t)AmclThread::uniform_pose_generator,
276 pf_->pop_err = pf_err_;
281 pf_vector_t pf_init_pose_mean = pf_vector_zero();
289 pf_init_pose_mean.v[0] = init_pose_[0];
290 pf_init_pose_mean.v[1] = init_pose_[1];
291 pf_init_pose_mean.v[2] = init_pose_[2];
293 pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
294 pf_init_pose_cov.m[0][0] = init_cov_[0];
295 pf_init_pose_cov.m[1][1] = init_cov_[1];
296 pf_init_pose_cov.m[2][2] = init_cov_[2];
297 pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);
301 initial_pose_hyp_->pf_pose_mean = pf_init_pose_mean;
302 initial_pose_hyp_->pf_pose_cov = pf_init_pose_cov;
306 odom_ = new ::amcl::AMCLOdom();
308 if (odom_model_type_ == ::amcl::ODOM_MODEL_OMNI)
309 odom_->SetModelOmni(alpha1_, alpha2_, alpha3_, alpha4_, alpha5_);
311 odom_->SetModelDiff(alpha1_, alpha2_, alpha3_, alpha4_);
314 laser_ = new ::amcl::AMCLLaser(max_beams_, map_);
316 if (laser_model_type_ == ::amcl::LASER_MODEL_BEAM) {
317 laser_->SetModelBeam(z_hit_, z_short_, z_max_, z_rand_, sigma_hit_,
321 "Initializing likelihood field model; " 322 "this can take some time on large maps...");
323 laser_->SetModelLikelihoodField(z_hit_, z_rand_, sigma_hit_,
324 laser_likelihood_max_dist_);
325 logger->
log_info(name(),
"Done initializing likelihood field model.");
335 bbil_add_message_interface(loc_if_);
339 laser_pose_set_ = set_laser_pose();
341 last_move_time_ =
new Time(clock);
342 last_move_time_->stamp();
344 cfg_buffer_enable_ =
true;
346 cfg_buffer_enable_ = config->
get_bool(AMCL_CFG_PREFIX
"buffering/enable");
349 cfg_buffer_debug_ =
true;
351 cfg_buffer_debug_ = config->
get_bool(AMCL_CFG_PREFIX
"buffering/debug");
354 laser_buffered_ =
false;
356 if (cfg_buffer_enable_) {
357 laser_if_->resize_buffers(1);
360 pos3d_if_->
set_frame(global_frame_id_.c_str());
363 char *map_file = strdup(cfg_map_file_.c_str());
364 std::string map_name = basename(map_file);
366 std::string::size_type pos;
367 if (((pos = map_name.rfind(
".")) != std::string::npos) && (pos > 0)) {
368 map_name = map_name.substr(0, pos-1);
371 loc_if_->set_map(map_name.c_str());
375 if (rt_) rt_->publish_map(global_frame_id_, map_);
378 apply_initial_pose();
387 if (!laser_pose_set_) {
388 if (set_laser_pose()) {
389 laser_pose_set_ =
true;
390 apply_initial_pose();
392 if (fawkes::runtime::uptime() >= tf_listener->get_cache_time()) {
393 logger->
log_warn(name(),
"Could not determine laser pose, skipping loop");
410 if (laser_if_->changed()) {
411 if (!get_odom_pose(odom_pose, pose.v[0], pose.v[1], pose.v[2],
412 laser_if_->timestamp(), base_frame_id_))
414 if (cfg_buffer_debug_) {
415 logger->
log_warn(name(),
"Couldn't determine robot's pose " 416 "associated with current laser scan");
418 if (laser_buffered_) {
419 Time buffer_timestamp(laser_if_->buffer_timestamp(0));
420 if (!get_odom_pose(odom_pose, pose.v[0], pose.v[1], pose.v[2],
421 &buffer_timestamp, base_frame_id_))
424 if (! get_odom_pose(odom_pose, pose.v[0], pose.v[1], pose.v[2],
425 &zero_time, base_frame_id_))
429 logger->
log_warn(name(),
"Couldn't determine robot's pose " 430 "associated with buffered laser scan nor at " 431 "current time, re-buffering");
432 laser_if_->copy_private_to_buffer(0);
437 laser_buffered_ =
false;
441 if (cfg_buffer_debug_) {
442 logger->
log_warn(name(),
"Using buffered laser data, re-buffering current");
444 laser_if_->read_from_buffer(0);
445 laser_if_->copy_shared_to_buffer(0);
447 }
else if (cfg_buffer_enable_) {
448 if (cfg_buffer_debug_) {
449 logger->
log_warn(name(),
"Buffering current data for next loop");
451 laser_if_->copy_private_to_buffer(0);
452 laser_buffered_ =
true;
459 laser_buffered_ =
false;
461 }
else if (laser_buffered_) {
463 laser_buffered_ =
false;
465 Time buffer_timestamp(laser_if_->buffer_timestamp(0));
466 if (get_odom_pose(odom_pose, pose.v[0], pose.v[1], pose.v[2],
467 &buffer_timestamp, base_frame_id_))
470 if (cfg_buffer_debug_) {
471 logger->
log_info(name(),
"Using buffered laser data (no changed data)");
473 laser_if_->read_from_buffer(0);
475 if (cfg_buffer_debug_) {
476 logger->
log_error(name(),
"Couldn't determine robot's pose " 477 "associated with buffered laser scan (2)");
486 float* laser_distances = laser_if_->distances();
488 pf_vector_t delta = pf_vector_zero();
493 delta.v[0] = pose.v[0] - pf_odom_pose_.v[0];
494 delta.v[1] = pose.v[1] - pf_odom_pose_.v[1];
495 delta.v[2] = angle_diff(pose.v[2], pf_odom_pose_.v[2]);
501 fabs(delta.v[0]) > d_thresh_ ||
502 fabs(delta.v[1]) > d_thresh_ ||
503 fabs(delta.v[2]) > a_thresh_;
506 last_move_time_->stamp();
516 laser_update_ =
true;
517 }
else if ((now - last_move_time_) <= t_thresh_) {
518 laser_update_ =
true;
522 bool force_publication =
false;
526 pf_odom_pose_ = pose;
532 laser_update_ =
true;
533 force_publication =
true;
536 }
else if (pf_init_ && laser_update_) {
541 ::amcl::AMCLOdomData odata;
550 odom_->UpdateAction(pf_, (::amcl::AMCLSensorData*) &odata);
556 bool resampled =
false;
561 ::amcl::AMCLLaserData ldata;
562 ldata.sensor = laser_;
563 ldata.range_count = angle_range_ + 1;
571 q.setEulerZYX(angle_min_, 0.0, 0.0);
573 q.setEulerZYX(angle_min_ + angle_increment_, 0.0, 0.0);
577 tf_listener->transform_quaternion(base_frame_id_, min_q, min_q);
578 tf_listener->transform_quaternion(base_frame_id_, inc_q, inc_q);
582 logger->
log_warn(name(),
"Unable to transform min/max laser angles " 588 double angle_min = tf::get_yaw(min_q);
589 double angle_increment = tf::get_yaw(inc_q) - angle_min;
592 angle_increment = fmod(angle_increment + 5 * M_PI, 2 * M_PI) - M_PI;
595 if (laser_max_range_ > 0.0)
596 ldata.range_max = (float) laser_max_range_;
598 ldata.range_max = std::numeric_limits<float>::max();
600 if (laser_min_range_ > 0.0)
601 range_min = (float) laser_min_range_;
605 ldata.ranges =
new double[ldata.range_count][2];
607 const unsigned int maxlen_dist = laser_if_->maxlenof_distances();
608 for (
int i = 0; i < ldata.range_count; ++i) {
609 unsigned int idx = (angle_min_idx_ + i) % maxlen_dist;
612 if (laser_distances[idx] <= range_min)
613 ldata.ranges[i][0] = ldata.range_max;
615 ldata.ranges[i][0] = laser_distances[idx];
618 ldata.ranges[i][1] = fmod(angle_min_ + (i * angle_increment), 2 * M_PI);
622 laser_->UpdateSensor(pf_, (::amcl::AMCLSensorData*) &ldata);
624 logger->
log_warn(name(),
"Failed to update laser sensor data, " 625 "exception follows");
629 laser_update_ =
false;
631 pf_odom_pose_ = pose;
634 if (!(++resample_count_ % resample_interval_)) {
636 pf_update_resample(pf_);
641 if (rt_) rt_->publish_pose_array(global_frame_id_, (pf_->sets) + pf_->current_set);
645 if (resampled || force_publication) {
647 double max_weight = 0.0;
648 int max_weight_hyp = -1;
649 std::vector<amcl_hyp_t> hyps;
650 hyps.resize(pf_->sets[pf_->current_set].cluster_count);
651 for (
int hyp_count = 0;
652 hyp_count < pf_->sets[pf_->current_set].cluster_count;
655 pf_vector_t pose_mean;
656 pf_matrix_t pose_cov;
657 if (!pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean,
659 logger->
log_error(name(),
"Couldn't get stats on cluster %d",
664 hyps[hyp_count].weight = weight;
665 hyps[hyp_count].pf_pose_mean = pose_mean;
666 hyps[hyp_count].pf_pose_cov = pose_cov;
668 if (hyps[hyp_count].weight > max_weight) {
669 max_weight = hyps[hyp_count].weight;
670 max_weight_hyp = hyp_count;
674 if (max_weight > 0.0) {
687 pf_sample_set_t*
set = pf_->sets + pf_->current_set;
688 for (
int i = 0; i < 2; i++) {
689 for (
int j = 0; j < 2; j++) {
693 last_covariance_[6 * i + j] =
set->cov.m[i][j];
700 last_covariance_[6 * 5 + 5] =
set->cov.m[2][2];
703 if (rt_) rt_->publish_pose(global_frame_id_, hyps[max_weight_hyp], last_covariance_);
718 tmp_tf(tf::create_quaternion_from_yaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
719 tf::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
720 hyps[max_weight_hyp].pf_pose_mean.v[1], 0.0));
723 latest, base_frame_id_);
724 tf_listener->transform_pose(odom_frame_id_,
725 tmp_tf_stamped, odom_to_map);
728 "Failed to subtract base to odom transform");
733 tf::Transform(tf::Quaternion(odom_to_map.getRotation()),
734 tf::Point(odom_to_map.getOrigin()));
735 latest_tf_valid_ =
true;
739 Time transform_expiration =
740 (*laser_if_->timestamp() + transform_tolerance_);
742 transform_expiration,
743 global_frame_id_, odom_frame_id_);
744 tf_publisher->send_transform(tmp_tf_stamped);
750 tf::Pose map_pose = latest_tf_.inverse() * odom_pose;
751 tf::Quaternion map_att = map_pose.getRotation();
753 double trans[3] = {map_pose.getOrigin().x(), map_pose.getOrigin().y(), 0};
754 double rot[4] = { map_att.x(), map_att.y(), map_att.z(), map_att.w() };
756 if (pos3d_if_->visibility_history() >= 0) {
757 pos3d_if_->set_visibility_history(pos3d_if_->visibility_history() + 1);
759 pos3d_if_->set_visibility_history(1);
761 pos3d_if_->set_translation(trans);
762 pos3d_if_->set_rotation(rot);
763 pos3d_if_->set_covariance(last_covariance_);
766 sent_first_transform_ =
true;
770 }
else if (latest_tf_valid_) {
773 Time transform_expiration =
774 (*laser_if_->timestamp() + transform_tolerance_);
776 transform_expiration,
777 global_frame_id_, odom_frame_id_);
778 tf_publisher->send_transform(tmp_tf_stamped);
783 tf::Pose map_pose = latest_tf_.inverse() * odom_pose;
784 tf::Quaternion map_att = map_pose.getRotation();
786 double trans[3] = {map_pose.getOrigin().x(), map_pose.getOrigin().y(), 0};
787 double rot[4] = { map_att.x(), map_att.y(), map_att.z(), map_att.w() };
789 if (pos3d_if_->visibility_history() >= 0) {
790 pos3d_if_->set_visibility_history(pos3d_if_->visibility_history() + 1);
792 pos3d_if_->set_visibility_history(1);
794 pos3d_if_->set_translation(trans);
795 pos3d_if_->set_rotation(rot);
800 if ((save_pose_period_ > 0.0) &&
801 (now - save_pose_last_time) >= save_pose_period_)
803 double yaw, pitch, roll;
804 map_pose.getBasis().getEulerYPR(yaw, pitch, roll);
812 config->
set_float(AMCL_CFG_PREFIX
"init_pose_x", map_pose.getOrigin().x());
813 config->
set_float(AMCL_CFG_PREFIX
"init_pose_y", map_pose.getOrigin().y());
814 config->
set_float(AMCL_CFG_PREFIX
"init_pose_a", yaw);
815 config->
set_float(AMCL_CFG_PREFIX
"init_cov_xx", last_covariance_[6 * 0 + 0]);
816 config->
set_float(AMCL_CFG_PREFIX
"init_cov_yy", last_covariance_[6 * 1 + 1]);
817 config->
set_float(AMCL_CFG_PREFIX
"init_cov_aa", last_covariance_[6 * 5 + 5]);
819 logger->
log_warn(name(),
"Failed to save pose, exception follows, disabling saving");
821 save_pose_period_ = 0.0;
824 save_pose_last_time = now;
827 if (pos3d_if_->visibility_history() <= 0) {
828 pos3d_if_->set_visibility_history(pos3d_if_->visibility_history() - 1);
830 pos3d_if_->set_visibility_history(-1);
839 bbil_remove_message_interface(loc_if_);
845 delete initial_pose_hyp_;
846 initial_pose_hyp_ = NULL;
848 delete last_move_time_;
850 blackboard->
close(laser_if_);
851 blackboard->
close(pos3d_if_);
852 blackboard->
close(loc_if_);
857 double& y,
double& yaw,
862 ident(tf::Transform(tf::Quaternion(0, 0, 0, 1),
863 tf::Vector3(0, 0, 0)), t, f);
865 tf_listener->transform_pose(odom_frame_id_, ident, odom_pose);
867 if (cfg_buffer_debug_) {
868 logger->
log_warn(name(),
"Failed to compute odom pose (%s)",
873 x = odom_pose.getOrigin().x();
874 y = odom_pose.getOrigin().y();
876 odom_pose.getBasis().getEulerZYX(yaw, pitch, roll);
885 AmclThread::set_laser_pose()
889 std::string laser_frame_id = laser_if_->frame();
890 if (laser_frame_id.empty())
return false;
894 ident(tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0, 0, 0)),
895 &now, laser_frame_id);
898 tf_listener->transform_pose(base_frame_id_, ident, laser_pose);
910 if (fawkes::runtime::uptime() >= tf_listener->get_cache_time()) {
911 logger->
log_error(name(),
"Generic exception for transform from %s to %s.",
912 laser_frame_id.c_str(), base_frame_id_.c_str());
935 pf_vector_t laser_pose_v;
936 laser_pose_v.v[0] = laser_pose.getOrigin().x();
937 laser_pose_v.v[1] = laser_pose.getOrigin().y();
940 laser_pose_v.v[2] = tf::get_yaw(laser_pose.getRotation());
941 laser_->SetLaserPose(laser_pose_v);
943 "Received laser's pose wrt robot: %.3f %.3f %.3f",
944 laser_pose_v.v[0], laser_pose_v.v[1], laser_pose_v.v[2]);
950 AmclThread::apply_initial_pose()
952 if (initial_pose_hyp_ != NULL && map_ != NULL) {
953 logger->
log_info(name(),
"Applying pose: %.3f %.3f %.3f " 954 "(cov: %.3f %.3f %.3f, %.3f %.3f %.3f, %.3f %.3f %.3f)",
955 initial_pose_hyp_->pf_pose_mean.v[0],
956 initial_pose_hyp_->pf_pose_mean.v[1],
957 initial_pose_hyp_->pf_pose_mean.v[2],
958 initial_pose_hyp_->pf_pose_cov.m[0][0],
959 initial_pose_hyp_->pf_pose_cov.m[0][1],
960 initial_pose_hyp_->pf_pose_cov.m[0][2],
961 initial_pose_hyp_->pf_pose_cov.m[1][0],
962 initial_pose_hyp_->pf_pose_cov.m[1][1],
963 initial_pose_hyp_->pf_pose_cov.m[1][2],
964 initial_pose_hyp_->pf_pose_cov.m[2][0],
965 initial_pose_hyp_->pf_pose_cov.m[2][1],
966 initial_pose_hyp_->pf_pose_cov.m[2][2]);
967 pf_init(pf_, initial_pose_hyp_->pf_pose_mean,
968 initial_pose_hyp_->pf_pose_cov);
971 logger->
log_warn(name(),
"Called apply initial pose but no pose to apply");
976 AmclThread::uniform_pose_generator(
void* arg)
978 map_t* map = (map_t*) arg;
979 #if NEW_UNIFORM_SAMPLING 980 unsigned int rand_index = drand48() * free_space_indices.size();
981 std::pair<int,int> free_point = free_space_indices[rand_index];
983 p.v[0] = MAP_WXGX(map, free_point.first);
984 p.v[1] = MAP_WYGY(map, free_point.second);
985 p.v[2] = drand48() * 2 * M_PI - M_PI;
987 double min_x, max_x, min_y, max_y;
988 min_x = (map->size_x * map->scale) / 2.0 - map->origin_x;
989 max_x = (map->size_x * map->scale) / 2.0 + map->origin_x;
990 min_y = (map->size_y * map->scale) / 2.0 - map->origin_y;
991 max_y = (map->size_y * map->scale) / 2.0 + map->origin_y;
995 p.v[0] = min_x + drand48() * (max_x - min_x);
996 p.v[1] = min_y + drand48() * (max_y - min_y);
997 p.v[2] = drand48() * 2 * M_PI - M_PI;
1000 i = MAP_GXWX(map, p.v[0]);
1001 j = MAP_GYWY(map, p.v[1]);
1002 if (MAP_VALID(map,i,j) && (map->cells[MAP_INDEX(map,i,j)].occ_state == -1))
1010 AmclThread::set_initial_pose(
const std::string &frame_id,
const fawkes::Time &msg_time,
1011 const tf::Pose &pose,
const double *covariance)
1014 if(frame_id ==
"") {
1016 logger->
log_warn(name(),
"Received initial pose with empty frame_id. " 1017 "You should always supply a frame_id.");
1018 }
else if (frame_id != global_frame_id_) {
1020 logger->
log_warn(name(),
"Ignoring initial pose in frame \"%s\"; " 1021 "initial poses must be in the global frame, \"%s\"",
1022 frame_id.c_str(), global_frame_id_.c_str());
1032 tf_listener->lookup_transform(base_frame_id_, latest,
1033 base_frame_id_, msg_time,
1034 global_frame_id_, tx_odom);
1040 if(sent_first_transform_)
1041 logger->
log_warn(name(),
"Failed to transform initial pose " 1043 tx_odom.setIdentity();
1045 logger->
log_warn(name(),
"Failed to transform initial pose in time");
1050 pose_new = tx_odom.inverse() * pose;
1054 logger->
log_info(name(),
"Setting pose: %.3f %.3f %.3f",
1055 pose_new.getOrigin().x(),
1056 pose_new.getOrigin().y(),
1057 tf::get_yaw(pose_new));
1059 pf_vector_t pf_init_pose_mean = pf_vector_zero();
1060 pf_init_pose_mean.v[0] = pose_new.getOrigin().x();
1061 pf_init_pose_mean.v[1] = pose_new.getOrigin().y();
1062 pf_init_pose_mean.v[2] = tf::get_yaw(pose_new);
1063 pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
1065 for(
int i=0; i<2; i++) {
1066 for(
int j=0; j<2; j++) {
1067 pf_init_pose_cov.m[i][j] = covariance[6*i+j];
1070 pf_init_pose_cov.m[2][2] = covariance[6*5+5];
1072 delete initial_pose_hyp_;
1074 initial_pose_hyp_->pf_pose_mean = pf_init_pose_mean;
1075 initial_pose_hyp_->pf_pose_cov = pf_init_pose_cov;
1076 apply_initial_pose();
1078 last_move_time_->stamp();
1084 AmclThread::bb_interface_message_received(
Interface *interface,
1099 const double *covariance = ipm->
covariance();
1100 set_initial_pose(ipm->
frame(), msg_time, pose, covariance);
Laser360Interface Fawkes BlackBoard Interface.
LocalizationInterface Fawkes BlackBoard Interface.
const Time * time_enqueued() const
Get time when message was enqueued.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
void set_frame(const char *new_frame)
Set frame value.
virtual void log_error(const char *component, const char *format,...)
Log error message.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
A class for handling time.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Thread class encapsulation of pthreads.
void write()
Write from local copy into BlackBoard memory.
Base class for all Fawkes BlackBoard interfaces.
sensor data processing thread
consider message received events
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.
double * covariance() const
Get covariance value.
double * translation() const
Get translation value.
virtual void init()
Initialize the thread.
Position3DInterface Fawkes BlackBoard Interface.
Base class for exceptions in Fawkes.
char * frame() const
Get frame value.
virtual void log_warn(const char *component, const char *format,...)
Log warning message.
SetInitialPoseMessage Fawkes BlackBoard Interface Message.
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
virtual void log_info(const char *component, const char *format,...)
Log informational message.
Wrapper class to add time stamp and frame ID to base types.
virtual void unlock()=0
Unlock the config.
virtual void finalize()
Finalize the thread.
virtual ~AmclThread()
Destructor.
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.
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
A frame could not be looked up.
Mutex mutual exclusion lock.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
double * rotation() const
Get rotation value.
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.
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.