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> 47 static double normalize(
double z) {
48 return atan2(sin(z), cos(z));
51 static double angle_diff(
double a,
double b) {
56 d2 = 2 * M_PI - fabs(d1);
59 if (fabs(d1) < fabs(d2))
70 std::vector<std::pair<int,int> > AmclThread::free_space_indices;
84 conf_mutex_ =
new Mutex();
100 fawkes::amcl::read_map_config(config, cfg_map_file_, cfg_resolution_, cfg_origin_x_,
101 cfg_origin_y_, cfg_origin_theta_, cfg_occupied_thresh_,
104 cfg_laser_ifname_ = config->
get_string(AMCL_CFG_PREFIX
"laser_interface_id");
105 cfg_pose_ifname_ = config->
get_string(AMCL_CFG_PREFIX
"pose_interface_id");
107 map_ = fawkes::amcl::read_map(cfg_map_file_.c_str(),
108 cfg_origin_x_, cfg_origin_y_, cfg_resolution_,
109 cfg_occupied_thresh_, cfg_free_thresh_, free_space_indices);
110 map_width_ = map_->size_x;
111 map_height_ = map_->size_y;
113 logger->
log_info(name(),
"Size: %ux%u (%zu of %u cells free, this are %.1f%%)",
114 map_width_, map_height_, free_space_indices.size(),
115 map_width_ * map_height_,
116 (float)free_space_indices.size() / (float)(map_width_ * map_height_) * 100.);
118 save_pose_last_time.set_clock(clock);
119 save_pose_last_time.stamp();
121 sent_first_transform_ =
false;
122 latest_tf_valid_ =
false;
128 initial_pose_hyp_ = NULL;
129 first_map_received_ =
false;
130 first_reconfigure_call_ =
true;
135 init_cov_[0] = 0.5 * 0.5;
136 init_cov_[1] = 0.5 * 0.5;
137 init_cov_[2] = (M_PI / 12.0) * (M_PI / 12.0);
139 save_pose_period_ = config->
get_float(AMCL_CFG_PREFIX
"save_pose_period");
140 laser_min_range_ = config->
get_float(AMCL_CFG_PREFIX
"laser_min_range");
141 laser_max_range_ = config->
get_float(AMCL_CFG_PREFIX
"laser_max_range");
142 pf_err_ = config->
get_float(AMCL_CFG_PREFIX
"kld_err");
143 pf_z_ = config->
get_float(AMCL_CFG_PREFIX
"kld_z");
144 alpha1_ = config->
get_float(AMCL_CFG_PREFIX
"alpha1");
145 alpha2_ = config->
get_float(AMCL_CFG_PREFIX
"alpha2");
146 alpha3_ = config->
get_float(AMCL_CFG_PREFIX
"alpha3");
147 alpha4_ = config->
get_float(AMCL_CFG_PREFIX
"alpha4");
148 alpha5_ = config->
get_float(AMCL_CFG_PREFIX
"alpha5");
149 z_hit_ = config->
get_float(AMCL_CFG_PREFIX
"z_hit");
150 z_short_ = config->
get_float(AMCL_CFG_PREFIX
"z_short");
151 z_max_ = config->
get_float(AMCL_CFG_PREFIX
"z_max");
152 z_rand_ = config->
get_float(AMCL_CFG_PREFIX
"z_rand");
153 sigma_hit_ = config->
get_float(AMCL_CFG_PREFIX
"sigma_hit");
154 lambda_short_ = config->
get_float(AMCL_CFG_PREFIX
"lambda_short");
155 laser_likelihood_max_dist_ =
156 config->
get_float(AMCL_CFG_PREFIX
"laser_likelihood_max_dist");
157 d_thresh_ = config->
get_float(AMCL_CFG_PREFIX
"d_thresh");
158 a_thresh_ = config->
get_float(AMCL_CFG_PREFIX
"a_thresh");
159 t_thresh_ = config->
get_float(AMCL_CFG_PREFIX
"t_thresh");
160 alpha_slow_ = config->
get_float(AMCL_CFG_PREFIX
"alpha_slow");
161 alpha_fast_ = config->
get_float(AMCL_CFG_PREFIX
"alpha_fast");
162 angle_increment_ =
deg2rad(config->
get_float(AMCL_CFG_PREFIX
"angle_increment"));
164 angle_min_idx_ = config->
get_uint(AMCL_CFG_PREFIX
"angle_min_idx");
165 if (angle_min_idx_ > 359) {
166 throw Exception(
"Angle min index out of bounds");
172 angle_max_idx_ = config->
get_uint(AMCL_CFG_PREFIX
"angle_max_idx");
173 if (angle_max_idx_ > 359) {
174 throw Exception(
"Angle max index out of bounds");
177 angle_max_idx_ = 359;
179 if (angle_max_idx_ > angle_min_idx_) {
180 angle_range_ = (
unsigned int)abs((
long)angle_max_idx_ - (long)angle_min_idx_);
182 angle_range_ = (360 - angle_min_idx_) + angle_max_idx_;
184 angle_min_ =
deg2rad(angle_min_idx_);
186 max_beams_ = config->
get_uint(AMCL_CFG_PREFIX
"max_beams");
187 min_particles_ = config->
get_uint(AMCL_CFG_PREFIX
"min_particles");
188 max_particles_ = config->
get_uint(AMCL_CFG_PREFIX
"max_particles");
189 resample_interval_ = config->
get_uint(AMCL_CFG_PREFIX
"resample_interval");
191 odom_frame_id_ = config->
get_string(
"/frames/odom");
192 base_frame_id_ = config->
get_string(
"/frames/base");
193 global_frame_id_ = config->
get_string(
"/frames/fixed");
195 tf_enable_publisher(odom_frame_id_.c_str());
197 std::string tmp_model_type;
198 tmp_model_type = config->
get_string(AMCL_CFG_PREFIX
"laser_model_type");
200 if (tmp_model_type ==
"beam")
201 laser_model_type_ = ::amcl::LASER_MODEL_BEAM;
202 else if (tmp_model_type ==
"likelihood_field")
203 laser_model_type_ = ::amcl::LASER_MODEL_LIKELIHOOD_FIELD;
206 "Unknown laser model type \"%s\"; " 207 "defaulting to likelihood_field model",
208 tmp_model_type.c_str());
209 laser_model_type_ = ::amcl::LASER_MODEL_LIKELIHOOD_FIELD;
212 tmp_model_type = config->
get_string(AMCL_CFG_PREFIX
"odom_model_type");
213 if (tmp_model_type ==
"diff")
214 odom_model_type_ = ::amcl::ODOM_MODEL_DIFF;
215 else if (tmp_model_type ==
"omni")
216 odom_model_type_ = ::amcl::ODOM_MODEL_OMNI;
219 "Unknown odom model type \"%s\"; defaulting to diff model",
220 tmp_model_type.c_str());
221 odom_model_type_ = ::amcl::ODOM_MODEL_DIFF;
225 init_pose_[0] = config->
get_float(AMCL_CFG_PREFIX
"init_pose_x");
229 init_pose_[1] = config->
get_float(AMCL_CFG_PREFIX
"init_pose_y");
233 init_pose_[2] = config->
get_float(AMCL_CFG_PREFIX
"init_pose_a");
236 cfg_read_init_cov_ =
false;
238 cfg_read_init_cov_ = config->
get_bool(AMCL_CFG_PREFIX
"read_init_cov");
241 if (cfg_read_init_cov_) {
243 init_cov_[0] = config->
get_float(AMCL_CFG_PREFIX
"init_cov_xx");
247 init_cov_[1] = config->
get_float(AMCL_CFG_PREFIX
"init_cov_yy");
251 init_cov_[2] = config->
get_float(AMCL_CFG_PREFIX
"init_cov_aa");
254 logger->
log_debug(name(),
"Reading initial covariance from config disabled");
257 transform_tolerance_ = config->
get_float(AMCL_CFG_PREFIX
"transform_tolerance");
259 if (min_particles_ > max_particles_) {
261 "You've set min_particles to be less than max particles, " 262 "this isn't allowed so they'll be set to be equal.");
263 max_particles_ = min_particles_;
268 pf_ = pf_alloc(min_particles_, max_particles_, alpha_slow_, alpha_fast_,
269 (pf_init_model_fn_t) AmclThread::uniform_pose_generator,
272 pf_init_model(pf_, (pf_init_model_fn_t)AmclThread::uniform_pose_generator,
275 pf_->pop_err = pf_err_;
280 pf_vector_t pf_init_pose_mean = pf_vector_zero();
288 pf_init_pose_mean.v[0] = init_pose_[0];
289 pf_init_pose_mean.v[1] = init_pose_[1];
290 pf_init_pose_mean.v[2] = init_pose_[2];
292 pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
293 pf_init_pose_cov.m[0][0] = init_cov_[0];
294 pf_init_pose_cov.m[1][1] = init_cov_[1];
295 pf_init_pose_cov.m[2][2] = init_cov_[2];
296 pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);
300 initial_pose_hyp_->pf_pose_mean = pf_init_pose_mean;
301 initial_pose_hyp_->pf_pose_cov = pf_init_pose_cov;
305 odom_ = new ::amcl::AMCLOdom();
307 if (odom_model_type_ == ::amcl::ODOM_MODEL_OMNI)
308 odom_->SetModelOmni(alpha1_, alpha2_, alpha3_, alpha4_, alpha5_);
310 odom_->SetModelDiff(alpha1_, alpha2_, alpha3_, alpha4_);
313 laser_ = new ::amcl::AMCLLaser(max_beams_, map_);
315 if (laser_model_type_ == ::amcl::LASER_MODEL_BEAM) {
316 laser_->SetModelBeam(z_hit_, z_short_, z_max_, z_rand_, sigma_hit_,
320 "Initializing likelihood field model; " 321 "this can take some time on large maps...");
322 laser_->SetModelLikelihoodField(z_hit_, z_rand_, sigma_hit_,
323 laser_likelihood_max_dist_);
324 logger->
log_info(name(),
"Done initializing likelihood field model.");
334 bbil_add_message_interface(loc_if_);
338 laser_pose_set_ = set_laser_pose();
340 last_move_time_ =
new Time(clock);
341 last_move_time_->stamp();
343 cfg_buffer_enable_ =
true;
345 cfg_buffer_enable_ = config->
get_bool(AMCL_CFG_PREFIX
"buffering/enable");
348 cfg_buffer_debug_ =
true;
350 cfg_buffer_debug_ = config->
get_bool(AMCL_CFG_PREFIX
"buffering/debug");
353 laser_buffered_ =
false;
355 if (cfg_buffer_enable_) {
356 laser_if_->resize_buffers(1);
359 pos3d_if_->
set_frame(global_frame_id_.c_str());
362 char *map_file = strdup(cfg_map_file_.c_str());
363 std::string map_name = basename(map_file);
365 std::string::size_type pos;
366 if (((pos = map_name.rfind(
".")) != std::string::npos) && (pos > 0)) {
367 map_name = map_name.substr(0, pos-1);
370 loc_if_->set_map(map_name.c_str());
374 if (rt_) rt_->publish_map(global_frame_id_, map_);
377 apply_initial_pose();
386 if (!laser_pose_set_) {
387 if (set_laser_pose()) {
388 laser_pose_set_ =
true;
389 apply_initial_pose();
391 if (fawkes::runtime::uptime() >= tf_listener->get_cache_time()) {
392 logger->
log_warn(name(),
"Could not determine laser pose, skipping loop");
409 if (laser_if_->changed()) {
410 if (!get_odom_pose(odom_pose, pose.v[0], pose.v[1], pose.v[2],
411 laser_if_->timestamp(), base_frame_id_))
413 if (cfg_buffer_debug_) {
414 logger->
log_warn(name(),
"Couldn't determine robot's pose " 415 "associated with current laser scan");
417 if (laser_buffered_) {
418 Time buffer_timestamp(laser_if_->buffer_timestamp(0));
419 if (!get_odom_pose(odom_pose, pose.v[0], pose.v[1], pose.v[2],
420 &buffer_timestamp, base_frame_id_))
423 if (! get_odom_pose(odom_pose, pose.v[0], pose.v[1], pose.v[2],
424 &zero_time, base_frame_id_))
428 logger->
log_warn(name(),
"Couldn't determine robot's pose " 429 "associated with buffered laser scan nor at " 430 "current time, re-buffering");
431 laser_if_->copy_private_to_buffer(0);
436 laser_buffered_ =
false;
440 if (cfg_buffer_debug_) {
441 logger->
log_warn(name(),
"Using buffered laser data, re-buffering current");
443 laser_if_->read_from_buffer(0);
444 laser_if_->copy_shared_to_buffer(0);
446 }
else if (cfg_buffer_enable_) {
447 if (cfg_buffer_debug_) {
448 logger->
log_warn(name(),
"Buffering current data for next loop");
450 laser_if_->copy_private_to_buffer(0);
451 laser_buffered_ =
true;
458 laser_buffered_ =
false;
460 }
else if (laser_buffered_) {
462 laser_buffered_ =
false;
464 Time buffer_timestamp(laser_if_->buffer_timestamp(0));
465 if (get_odom_pose(odom_pose, pose.v[0], pose.v[1], pose.v[2],
466 &buffer_timestamp, base_frame_id_))
469 if (cfg_buffer_debug_) {
470 logger->
log_info(name(),
"Using buffered laser data (no changed data)");
472 laser_if_->read_from_buffer(0);
474 if (cfg_buffer_debug_) {
475 logger->
log_error(name(),
"Couldn't determine robot's pose " 476 "associated with buffered laser scan (2)");
485 float* laser_distances = laser_if_->distances();
487 pf_vector_t delta = pf_vector_zero();
492 delta.v[0] = pose.v[0] - pf_odom_pose_.v[0];
493 delta.v[1] = pose.v[1] - pf_odom_pose_.v[1];
494 delta.v[2] = angle_diff(pose.v[2], pf_odom_pose_.v[2]);
500 fabs(delta.v[0]) > d_thresh_ ||
501 fabs(delta.v[1]) > d_thresh_ ||
502 fabs(delta.v[2]) > a_thresh_;
505 last_move_time_->stamp();
515 laser_update_ =
true;
516 }
else if ((now - last_move_time_) <= t_thresh_) {
517 laser_update_ =
true;
521 bool force_publication =
false;
525 pf_odom_pose_ = pose;
531 laser_update_ =
true;
532 force_publication =
true;
535 }
else if (pf_init_ && laser_update_) {
540 ::amcl::AMCLOdomData odata;
549 odom_->UpdateAction(pf_, (::amcl::AMCLSensorData*) &odata);
555 bool resampled =
false;
560 ::amcl::AMCLLaserData ldata;
561 ldata.sensor = laser_;
562 ldata.range_count = angle_range_ + 1;
570 q.setEulerZYX(angle_min_, 0.0, 0.0);
572 q.setEulerZYX(angle_min_ + angle_increment_, 0.0, 0.0);
576 tf_listener->transform_quaternion(base_frame_id_, min_q, min_q);
577 tf_listener->transform_quaternion(base_frame_id_, inc_q, inc_q);
581 logger->
log_warn(name(),
"Unable to transform min/max laser angles " 587 double angle_min = tf::get_yaw(min_q);
588 double angle_increment = tf::get_yaw(inc_q) - angle_min;
591 angle_increment = fmod(angle_increment + 5 * M_PI, 2 * M_PI) - M_PI;
594 if (laser_max_range_ > 0.0)
595 ldata.range_max = (float) laser_max_range_;
597 ldata.range_max = HUGE;
599 if (laser_min_range_ > 0.0)
600 range_min = (float) laser_min_range_;
604 ldata.ranges =
new double[ldata.range_count][2];
606 const unsigned int maxlen_dist = laser_if_->maxlenof_distances();
607 for (
int i = 0; i < ldata.range_count; ++i) {
608 unsigned int idx = (angle_min_idx_ + i) % maxlen_dist;
611 if (laser_distances[idx] <= range_min)
612 ldata.ranges[i][0] = ldata.range_max;
614 ldata.ranges[i][0] = laser_distances[idx];
617 ldata.ranges[i][1] = fmod(angle_min_ + (i * angle_increment), 2 * M_PI);
621 laser_->UpdateSensor(pf_, (::amcl::AMCLSensorData*) &ldata);
623 logger->
log_warn(name(),
"Failed to update laser sensor data, " 624 "exception follows");
628 laser_update_ =
false;
630 pf_odom_pose_ = pose;
633 if (!(++resample_count_ % resample_interval_)) {
635 pf_update_resample(pf_);
640 if (rt_) rt_->publish_pose_array(global_frame_id_, (pf_->sets) + pf_->current_set);
644 if (resampled || force_publication) {
646 double max_weight = 0.0;
647 int max_weight_hyp = -1;
648 std::vector<amcl_hyp_t> hyps;
649 hyps.resize(pf_->sets[pf_->current_set].cluster_count);
650 for (
int hyp_count = 0;
651 hyp_count < pf_->sets[pf_->current_set].cluster_count;
654 pf_vector_t pose_mean;
655 pf_matrix_t pose_cov;
656 if (!pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean,
658 logger->
log_error(name(),
"Couldn't get stats on cluster %d",
663 hyps[hyp_count].weight = weight;
664 hyps[hyp_count].pf_pose_mean = pose_mean;
665 hyps[hyp_count].pf_pose_cov = pose_cov;
667 if (hyps[hyp_count].weight > max_weight) {
668 max_weight = hyps[hyp_count].weight;
669 max_weight_hyp = hyp_count;
673 if (max_weight > 0.0) {
686 pf_sample_set_t*
set = pf_->sets + pf_->current_set;
687 for (
int i = 0; i < 2; i++) {
688 for (
int j = 0; j < 2; j++) {
692 last_covariance_[6 * i + j] =
set->cov.m[i][j];
699 last_covariance_[6 * 5 + 5] =
set->cov.m[2][2];
702 if (rt_) rt_->publish_pose(global_frame_id_, hyps[max_weight_hyp], last_covariance_);
717 tmp_tf(tf::create_quaternion_from_yaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
718 tf::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
719 hyps[max_weight_hyp].pf_pose_mean.v[1], 0.0));
722 latest, base_frame_id_);
723 tf_listener->transform_pose(odom_frame_id_,
724 tmp_tf_stamped, odom_to_map);
727 "Failed to subtract base to odom transform");
732 tf::Transform(tf::Quaternion(odom_to_map.getRotation()),
733 tf::Point(odom_to_map.getOrigin()));
734 latest_tf_valid_ =
true;
738 Time transform_expiration =
739 (*laser_if_->timestamp() + transform_tolerance_);
741 transform_expiration,
742 global_frame_id_, odom_frame_id_);
743 tf_publisher->send_transform(tmp_tf_stamped);
749 tf::Pose map_pose = latest_tf_.inverse() * odom_pose;
750 tf::Quaternion map_att = map_pose.getRotation();
752 double trans[3] = {map_pose.getOrigin().x(), map_pose.getOrigin().y(), 0};
753 double rot[4] = { map_att.x(), map_att.y(), map_att.z(), map_att.w() };
755 if (pos3d_if_->visibility_history() >= 0) {
756 pos3d_if_->set_visibility_history(pos3d_if_->visibility_history() + 1);
758 pos3d_if_->set_visibility_history(1);
760 pos3d_if_->set_translation(trans);
761 pos3d_if_->set_rotation(rot);
762 pos3d_if_->set_covariance(last_covariance_);
765 sent_first_transform_ =
true;
769 }
else if (latest_tf_valid_) {
772 Time transform_expiration =
773 (*laser_if_->timestamp() + transform_tolerance_);
775 transform_expiration,
776 global_frame_id_, odom_frame_id_);
777 tf_publisher->send_transform(tmp_tf_stamped);
782 tf::Pose map_pose = latest_tf_.inverse() * odom_pose;
783 tf::Quaternion map_att = map_pose.getRotation();
785 double trans[3] = {map_pose.getOrigin().x(), map_pose.getOrigin().y(), 0};
786 double rot[4] = { map_att.x(), map_att.y(), map_att.z(), map_att.w() };
788 if (pos3d_if_->visibility_history() >= 0) {
789 pos3d_if_->set_visibility_history(pos3d_if_->visibility_history() + 1);
791 pos3d_if_->set_visibility_history(1);
793 pos3d_if_->set_translation(trans);
794 pos3d_if_->set_rotation(rot);
799 if ((save_pose_period_ > 0.0) &&
800 (now - save_pose_last_time) >= save_pose_period_)
802 double yaw, pitch, roll;
803 map_pose.getBasis().getEulerYPR(yaw, pitch, roll);
811 config->
set_float(AMCL_CFG_PREFIX
"init_pose_x", map_pose.getOrigin().x());
812 config->
set_float(AMCL_CFG_PREFIX
"init_pose_y", map_pose.getOrigin().y());
813 config->
set_float(AMCL_CFG_PREFIX
"init_pose_a", yaw);
814 config->
set_float(AMCL_CFG_PREFIX
"init_cov_xx", last_covariance_[6 * 0 + 0]);
815 config->
set_float(AMCL_CFG_PREFIX
"init_cov_yy", last_covariance_[6 * 1 + 1]);
816 config->
set_float(AMCL_CFG_PREFIX
"init_cov_aa", last_covariance_[6 * 5 + 5]);
818 logger->
log_warn(name(),
"Failed to save pose, exception follows, disabling saving");
820 save_pose_period_ = 0.0;
823 save_pose_last_time = now;
826 if (pos3d_if_->visibility_history() <= 0) {
827 pos3d_if_->set_visibility_history(pos3d_if_->visibility_history() - 1);
829 pos3d_if_->set_visibility_history(-1);
838 bbil_remove_message_interface(loc_if_);
844 delete initial_pose_hyp_;
845 initial_pose_hyp_ = NULL;
847 delete last_move_time_;
849 blackboard->
close(laser_if_);
850 blackboard->
close(pos3d_if_);
851 blackboard->
close(loc_if_);
856 double& y,
double& yaw,
861 ident(tf::Transform(tf::Quaternion(0, 0, 0, 1),
862 tf::Vector3(0, 0, 0)), t, f);
864 tf_listener->transform_pose(odom_frame_id_, ident, odom_pose);
866 if (cfg_buffer_debug_) {
867 logger->
log_warn(name(),
"Failed to compute odom pose (%s)",
872 x = odom_pose.getOrigin().x();
873 y = odom_pose.getOrigin().y();
875 odom_pose.getBasis().getEulerZYX(yaw, pitch, roll);
884 AmclThread::set_laser_pose()
888 std::string laser_frame_id = laser_if_->frame();
889 if (laser_frame_id.empty())
return false;
893 ident(tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0, 0, 0)),
894 &now, laser_frame_id);
897 tf_listener->transform_pose(base_frame_id_, ident, laser_pose);
909 if (fawkes::runtime::uptime() >= tf_listener->get_cache_time()) {
910 logger->
log_error(name(),
"Generic exception for transform from %s to %s.",
911 laser_frame_id.c_str(), base_frame_id_.c_str());
934 pf_vector_t laser_pose_v;
935 laser_pose_v.v[0] = laser_pose.getOrigin().x();
936 laser_pose_v.v[1] = laser_pose.getOrigin().y();
939 laser_pose_v.v[2] = tf::get_yaw(laser_pose.getRotation());
940 laser_->SetLaserPose(laser_pose_v);
942 "Received laser's pose wrt robot: %.3f %.3f %.3f",
943 laser_pose_v.v[0], laser_pose_v.v[1], laser_pose_v.v[2]);
949 AmclThread::apply_initial_pose()
951 if (initial_pose_hyp_ != NULL && map_ != NULL) {
952 logger->
log_info(name(),
"Applying pose: %.3f %.3f %.3f " 953 "(cov: %.3f %.3f %.3f, %.3f %.3f %.3f, %.3f %.3f %.3f)",
954 initial_pose_hyp_->pf_pose_mean.v[0],
955 initial_pose_hyp_->pf_pose_mean.v[1],
956 initial_pose_hyp_->pf_pose_mean.v[2],
957 initial_pose_hyp_->pf_pose_cov.m[0][0],
958 initial_pose_hyp_->pf_pose_cov.m[0][1],
959 initial_pose_hyp_->pf_pose_cov.m[0][2],
960 initial_pose_hyp_->pf_pose_cov.m[1][0],
961 initial_pose_hyp_->pf_pose_cov.m[1][1],
962 initial_pose_hyp_->pf_pose_cov.m[1][2],
963 initial_pose_hyp_->pf_pose_cov.m[2][0],
964 initial_pose_hyp_->pf_pose_cov.m[2][1],
965 initial_pose_hyp_->pf_pose_cov.m[2][2]);
966 pf_init(pf_, initial_pose_hyp_->pf_pose_mean,
967 initial_pose_hyp_->pf_pose_cov);
970 logger->
log_warn(name(),
"Called apply initial pose but no pose to apply");
975 AmclThread::uniform_pose_generator(
void* arg)
977 map_t* map = (map_t*) arg;
978 #if NEW_UNIFORM_SAMPLING 979 unsigned int rand_index = drand48() * free_space_indices.size();
980 std::pair<int,int> free_point = free_space_indices[rand_index];
982 p.v[0] = MAP_WXGX(map, free_point.first);
983 p.v[1] = MAP_WYGY(map, free_point.second);
984 p.v[2] = drand48() * 2 * M_PI - M_PI;
986 double min_x, max_x, min_y, max_y;
987 min_x = (map->size_x * map->scale) / 2.0 - map->origin_x;
988 max_x = (map->size_x * map->scale) / 2.0 + map->origin_x;
989 min_y = (map->size_y * map->scale) / 2.0 - map->origin_y;
990 max_y = (map->size_y * map->scale) / 2.0 + map->origin_y;
994 p.v[0] = min_x + drand48() * (max_x - min_x);
995 p.v[1] = min_y + drand48() * (max_y - min_y);
996 p.v[2] = drand48() * 2 * M_PI - M_PI;
999 i = MAP_GXWX(map, p.v[0]);
1000 j = MAP_GYWY(map, p.v[1]);
1001 if (MAP_VALID(map,i,j) && (map->cells[MAP_INDEX(map,i,j)].occ_state == -1))
1009 AmclThread::set_initial_pose(
const std::string &frame_id,
const fawkes::Time &msg_time,
1010 const tf::Pose &pose,
const double *covariance)
1013 if(frame_id ==
"") {
1015 logger->
log_warn(name(),
"Received initial pose with empty frame_id. " 1016 "You should always supply a frame_id.");
1017 }
else if (frame_id != global_frame_id_) {
1019 logger->
log_warn(name(),
"Ignoring initial pose in frame \"%s\"; " 1020 "initial poses must be in the global frame, \"%s\"",
1021 frame_id.c_str(), global_frame_id_.c_str());
1031 tf_listener->lookup_transform(base_frame_id_, latest,
1032 base_frame_id_, msg_time,
1033 global_frame_id_, tx_odom);
1039 if(sent_first_transform_)
1040 logger->
log_warn(name(),
"Failed to transform initial pose " 1042 tx_odom.setIdentity();
1044 logger->
log_warn(name(),
"Failed to transform initial pose in time");
1049 pose_new = tx_odom.inverse() * pose;
1053 logger->
log_info(name(),
"Setting pose: %.3f %.3f %.3f",
1054 pose_new.getOrigin().x(),
1055 pose_new.getOrigin().y(),
1056 tf::get_yaw(pose_new));
1058 pf_vector_t pf_init_pose_mean = pf_vector_zero();
1059 pf_init_pose_mean.v[0] = pose_new.getOrigin().x();
1060 pf_init_pose_mean.v[1] = pose_new.getOrigin().y();
1061 pf_init_pose_mean.v[2] = tf::get_yaw(pose_new);
1062 pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
1064 for(
int i=0; i<2; i++) {
1065 for(
int j=0; j<2; j++) {
1066 pf_init_pose_cov.m[i][j] = covariance[6*i+j];
1069 pf_init_pose_cov.m[2][2] = covariance[6*5+5];
1071 delete initial_pose_hyp_;
1073 initial_pose_hyp_->pf_pose_mean = pf_init_pose_mean;
1074 initial_pose_hyp_->pf_pose_cov = pf_init_pose_cov;
1075 apply_initial_pose();
1077 last_move_time_->stamp();
1083 AmclThread::bb_interface_message_received(
Interface *interface,
1098 const double *covariance = ipm->
covariance();
1099 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.