Fawkes API
Fawkes Development Version
|
00001 /*************************************************************************** 00002 * amcl_thread.cpp - Thread to perform localization 00003 * 00004 * Created: Wed May 16 16:04:41 2012 00005 * Copyright 2012 Tim Niemueller [www.niemueller.de] 00006 * 2012 Daniel Ewert 00007 * 2012 Kathrin Goffart (Robotino Hackathon 2012) 00008 * 2012 Kilian Hinterwaelder (Robotino Hackathon 2012) 00009 * 2012 Marcel Prochnau (Robotino Hackathon 2012) 00010 * 2012 Jannik Altgen (Robotino Hackathon 2012) 00011 ****************************************************************************/ 00012 00013 /* This program is free software; you can redistribute it and/or modify 00014 * it under the terms of the GNU General Public License as published by 00015 * the Free Software Foundation; either version 2 of the License, or 00016 * (at your option) any later version. 00017 * 00018 * This program is distributed in the hope that it will be useful, 00019 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00020 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00021 * GNU Library General Public License for more details. 00022 * 00023 * Read the full text in the LICENSE.GPL file in the doc directory. 00024 */ 00025 00026 /* Based on amcl_node.cpp from the ROS amcl package 00027 * Copyright (c) 2008, Willow Garage, Inc. 00028 */ 00029 00030 #include "amcl_thread.h" 00031 #include "amcl_utils.h" 00032 00033 #include <utils/math/angle.h> 00034 #include <core/threading/mutex.h> 00035 #include <core/threading/mutex_locker.h> 00036 #include <cstdlib> 00037 #include <cstdio> 00038 00039 #ifdef HAVE_ROS 00040 # include <ros/node_handle.h> 00041 # include <geometry_msgs/PoseArray.h> 00042 # ifdef USE_MAP_PUB 00043 # include <nav_msgs/OccupancyGrid.h> 00044 # endif 00045 #endif 00046 00047 using namespace fawkes; 00048 00049 static double normalize(double z) { 00050 return atan2(sin(z), cos(z)); 00051 } 00052 00053 static double angle_diff(double a, double b) { 00054 double d1, d2; 00055 a = normalize(a); 00056 b = normalize(b); 00057 d1 = a - b; 00058 d2 = 2 * M_PI - fabs(d1); 00059 if (d1 > 0) 00060 d2 *= -1.0; 00061 if (fabs(d1) < fabs(d2)) 00062 return (d1); 00063 else 00064 return (d2); 00065 } 00066 00067 /** @class AmclThread "amcl_thread.h" 00068 * Thread to perform Adaptive Monte Carlo Localization. 00069 * @author Tim Niemueller 00070 */ 00071 00072 std::vector<std::pair<int,int> > AmclThread::free_space_indices; 00073 00074 /** Constructor. */ 00075 AmclThread::AmclThread() 00076 : Thread("AmclThread", Thread::OPMODE_WAITFORWAKEUP), 00077 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS), 00078 TransformAspect(TransformAspect::BOTH, "Pose") 00079 { 00080 map_ = NULL; 00081 conf_mutex_ = new Mutex(); 00082 } 00083 00084 /** Destructor. */ 00085 AmclThread::~AmclThread() 00086 { 00087 delete conf_mutex_; 00088 } 00089 00090 void AmclThread::init() 00091 { 00092 fawkes::amcl::read_map_config(config, cfg_map_file_, cfg_resolution_, cfg_origin_x_, 00093 cfg_origin_y_, cfg_origin_theta_, cfg_occupied_thresh_, 00094 cfg_free_thresh_); 00095 00096 cfg_laser_ifname_ = config->get_string(CFG_PREFIX"laser_interface_id"); 00097 cfg_pose_ifname_ = config->get_string(CFG_PREFIX"pose_interface_id"); 00098 00099 map_ = fawkes::amcl::read_map(cfg_map_file_.c_str(), 00100 cfg_origin_x_, cfg_origin_y_, cfg_resolution_, 00101 cfg_occupied_thresh_, cfg_free_thresh_, free_space_indices); 00102 map_width_ = map_->size_x; 00103 map_height_ = map_->size_y; 00104 00105 logger->log_info(name(), "Size: %ux%u (%zu of %u cells free, this are %.1f%%)", 00106 map_width_, map_height_, free_space_indices.size(), 00107 map_width_ * map_height_, 00108 (float)free_space_indices.size() / (float)(map_width_ * map_height_) * 100.); 00109 00110 sent_first_transform_ = false; 00111 latest_tf_valid_ = false; 00112 pf_ = NULL; 00113 resample_count_ = 0; 00114 odom_ = NULL; 00115 laser_ = NULL; 00116 // private_nh_="~"; 00117 initial_pose_hyp_ = NULL; 00118 first_map_received_ = false; 00119 first_reconfigure_call_ = true; 00120 00121 init_pose_[0] = 0.0; 00122 init_pose_[1] = 0.0; 00123 init_pose_[2] = 0.0; 00124 init_cov_[0] = 0.5 * 0.5; 00125 init_cov_[1] = 0.5 * 0.5; 00126 init_cov_[2] = (M_PI / 12.0) * (M_PI / 12.0); 00127 00128 save_pose_period = config->get_float(CFG_PREFIX"save_pose_rate"); 00129 laser_min_range_ = config->get_float(CFG_PREFIX"laser_min_range"); 00130 laser_max_range_ = config->get_float(CFG_PREFIX"laser_max_range"); 00131 pf_err_ = config->get_float(CFG_PREFIX"kld_err"); 00132 pf_z_ = config->get_float(CFG_PREFIX"kld_z"); 00133 alpha1_ = config->get_float(CFG_PREFIX"alpha1"); 00134 alpha2_ = config->get_float(CFG_PREFIX"alpha2"); 00135 alpha3_ = config->get_float(CFG_PREFIX"alpha3"); 00136 alpha4_ = config->get_float(CFG_PREFIX"alpha4"); 00137 alpha5_ = config->get_float(CFG_PREFIX"alpha5"); 00138 z_hit_ = config->get_float(CFG_PREFIX"z_hit"); 00139 z_short_ = config->get_float(CFG_PREFIX"z_short"); 00140 z_max_ = config->get_float(CFG_PREFIX"z_max"); 00141 z_rand_ = config->get_float(CFG_PREFIX"z_rand"); 00142 sigma_hit_ = config->get_float(CFG_PREFIX"sigma_hit"); 00143 lambda_short_ = config->get_float(CFG_PREFIX"lambda_short"); 00144 laser_likelihood_max_dist_ = 00145 config->get_float(CFG_PREFIX"laser_likelihood_max_dist"); 00146 d_thresh_ = config->get_float(CFG_PREFIX"d_thresh"); 00147 a_thresh_ = config->get_float(CFG_PREFIX"a_thresh"); 00148 t_thresh_ = config->get_float(CFG_PREFIX"t_thresh"); 00149 alpha_slow_ = config->get_float(CFG_PREFIX"alpha_slow"); 00150 alpha_fast_ = config->get_float(CFG_PREFIX"alpha_fast"); 00151 angle_increment_ = deg2rad(config->get_float(CFG_PREFIX"angle_increment")); 00152 try { 00153 angle_min_idx_ = config->get_uint(CFG_PREFIX"angle_min_idx"); 00154 if (angle_min_idx_ > 359) { 00155 throw Exception("Angle min index out of bounds"); 00156 } 00157 } catch (Exception &e) { 00158 angle_min_idx_ = 0; 00159 } 00160 try { 00161 angle_max_idx_ = config->get_uint(CFG_PREFIX"angle_max_idx"); 00162 if (angle_max_idx_ > 359) { 00163 throw Exception("Angle max index out of bounds"); 00164 } 00165 } catch (Exception &e) { 00166 angle_max_idx_ = 359; 00167 } 00168 angle_range_ = (unsigned int)abs(angle_max_idx_ - angle_min_idx_); 00169 angle_min_ = deg2rad(angle_min_idx_); 00170 00171 max_beams_ = config->get_uint(CFG_PREFIX"max_beams"); 00172 min_particles_ = config->get_uint(CFG_PREFIX"min_particles"); 00173 max_particles_ = config->get_uint(CFG_PREFIX"max_particles"); 00174 resample_interval_ = config->get_uint(CFG_PREFIX"resample_interval"); 00175 00176 odom_frame_id_ = config->get_string(CFG_PREFIX"odom_frame_id"); 00177 base_frame_id_ = config->get_string(CFG_PREFIX"base_frame_id"); 00178 laser_frame_id_ = config->get_string(CFG_PREFIX"laser_frame_id"); 00179 global_frame_id_ = config->get_string(CFG_PREFIX"global_frame_id"); 00180 00181 std::string tmp_model_type; 00182 tmp_model_type = config->get_string(CFG_PREFIX"laser_model_type"); 00183 00184 if (tmp_model_type == "beam") 00185 laser_model_type_ = ::amcl::LASER_MODEL_BEAM; 00186 else if (tmp_model_type == "likelihood_field") 00187 laser_model_type_ = ::amcl::LASER_MODEL_LIKELIHOOD_FIELD; 00188 else { 00189 logger->log_warn(name(), 00190 "Unknown laser model type \"%s\"; " 00191 "defaulting to likelihood_field model", 00192 tmp_model_type.c_str()); 00193 laser_model_type_ = ::amcl::LASER_MODEL_LIKELIHOOD_FIELD; 00194 } 00195 00196 tmp_model_type = config->get_string(CFG_PREFIX"odom_model_type"); 00197 if (tmp_model_type == "diff") 00198 odom_model_type_ = ::amcl::ODOM_MODEL_DIFF; 00199 else if (tmp_model_type == "omni") 00200 odom_model_type_ = ::amcl::ODOM_MODEL_OMNI; 00201 else { 00202 logger->log_warn(name(), 00203 "Unknown odom model type \"%s\"; defaulting to diff model", 00204 tmp_model_type.c_str()); 00205 odom_model_type_ = ::amcl::ODOM_MODEL_DIFF; 00206 } 00207 00208 try { 00209 init_pose_[0] = config->get_float(CFG_PREFIX"init_pose_x"); 00210 } catch (Exception &e) {} // ignored, use default 00211 00212 try { 00213 init_pose_[1] = config->get_float(CFG_PREFIX"init_pose_y"); 00214 } catch (Exception &e) {} // ignored, use default 00215 00216 try { 00217 init_pose_[2] = config->get_float(CFG_PREFIX"init_pose_a"); 00218 } catch (Exception &e) {} // ignored, use default 00219 00220 try { 00221 init_cov_[0] = config->get_float(CFG_PREFIX"init_cov_xx"); 00222 } catch (Exception &e) {} // ignored, use default 00223 00224 try { 00225 init_cov_[1] = config->get_float(CFG_PREFIX"init_cov_yy"); 00226 } catch (Exception &e) {} // ignored, use default 00227 00228 try { 00229 init_cov_[2] = config->get_float(CFG_PREFIX"init_cov_aa"); 00230 } catch (Exception &e) {} // ignored, use default 00231 00232 transform_tolerance_ = config->get_float(CFG_PREFIX"transform_tolerance"); 00233 00234 if (min_particles_ > max_particles_) { 00235 logger->log_warn(name(), 00236 "You've set min_particles to be less than max particles, " 00237 "this isn't allowed so they'll be set to be equal."); 00238 max_particles_ = min_particles_; 00239 } 00240 00241 //logger->log_info(name(),"calling pf_alloc with %d min and %d max particles", 00242 // min_particles_, max_particles_); 00243 pf_ = pf_alloc(min_particles_, max_particles_, alpha_slow_, alpha_fast_, 00244 (pf_init_model_fn_t) AmclThread::uniform_pose_generator, 00245 (void *) map_); 00246 00247 pf_init_model(pf_, (pf_init_model_fn_t)AmclThread::uniform_pose_generator, 00248 (void *)map_); 00249 00250 pf_->pop_err = pf_err_; 00251 pf_->pop_z = pf_z_; 00252 00253 // Initialize the filter 00254 00255 pf_vector_t pf_init_pose_mean = pf_vector_zero(); 00256 //pf_init_pose_mean.v[0] = last_published_pose.pose.pose.position.x; 00257 //pf_init_pose_mean.v[1] = last_published_pose.pose.pose.position.y; 00258 //double *q_values = pos3d_if_->rotation(); 00259 //tf::Quaternion q(q_values[0], q_values[1], q_values[2], q_values[3]); 00260 //btScalar unused_pitch, unused_roll, yaw; 00261 //btMatrix3x3(q).getRPY(unused_roll, unused_pitch, yaw); 00262 00263 pf_init_pose_mean.v[0] = init_pose_[0]; 00264 pf_init_pose_mean.v[1] = init_pose_[1]; 00265 pf_init_pose_mean.v[2] = init_pose_[2]; 00266 00267 pf_matrix_t pf_init_pose_cov = pf_matrix_zero(); 00268 pf_init_pose_cov.m[0][0] = init_cov_[0]; //last_covariance_[6 * 0 + 0]; 00269 pf_init_pose_cov.m[1][1] = init_cov_[1]; //last_covariance_[6 * 1 + 1]; 00270 pf_init_pose_cov.m[2][2] = init_cov_[2]; //last_covariance_[6 * 5 + 5]; 00271 pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov); 00272 pf_init_ = false; 00273 00274 initial_pose_hyp_ = new amcl_hyp_t(); 00275 initial_pose_hyp_->pf_pose_mean = pf_init_pose_mean; 00276 initial_pose_hyp_->pf_pose_cov = pf_init_pose_cov; 00277 00278 // Instantiate the sensor objects 00279 // Odometry 00280 odom_ = new ::amcl::AMCLOdom(); 00281 00282 if (odom_model_type_ == ::amcl::ODOM_MODEL_OMNI) 00283 odom_->SetModelOmni(alpha1_, alpha2_, alpha3_, alpha4_, alpha5_); 00284 else 00285 odom_->SetModelDiff(alpha1_, alpha2_, alpha3_, alpha4_); 00286 00287 // Laser 00288 laser_ = new ::amcl::AMCLLaser(max_beams_, map_); 00289 00290 if (laser_model_type_ == ::amcl::LASER_MODEL_BEAM) { 00291 laser_->SetModelBeam(z_hit_, z_short_, z_max_, z_rand_, sigma_hit_, 00292 lambda_short_, 0.0); 00293 } else { 00294 logger->log_info(name(), 00295 "Initializing likelihood field model; " 00296 "this can take some time on large maps..."); 00297 laser_->SetModelLikelihoodField(z_hit_, z_rand_, sigma_hit_, 00298 laser_likelihood_max_dist_); 00299 logger->log_info(name(), "Done initializing likelihood field model."); 00300 } 00301 00302 laser_pose_set_ = set_laser_pose(); 00303 00304 last_move_time_ = new Time(clock); 00305 last_move_time_->stamp(); 00306 00307 #ifdef HAVE_ROS 00308 pose_pub_ = 00309 rosnode->advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose", 2); 00310 particlecloud_pub_ = 00311 rosnode->advertise<geometry_msgs::PoseArray>("particlecloud", 2); 00312 initial_pose_sub_ = 00313 rosnode->subscribe("initialpose", 2, 00314 &AmclThread::initial_pose_received, this); 00315 # ifdef USE_MAP_PUB 00316 map_pub_ = rosnode->advertise<nav_msgs::OccupancyGrid>("map", 1, true); 00317 publish_map(); 00318 # endif 00319 #endif 00320 00321 laser_if_ = 00322 blackboard->open_for_reading<Laser360Interface>(cfg_laser_ifname_.c_str()); 00323 pos3d_if_ = 00324 blackboard->open_for_writing<Position3DInterface>(cfg_pose_ifname_.c_str()); 00325 00326 pos3d_if_->set_frame(global_frame_id_.c_str()); 00327 pos3d_if_->write(); 00328 00329 apply_initial_pose(); 00330 } 00331 00332 00333 void 00334 AmclThread::loop() 00335 { 00336 if (!laser_pose_set_) { 00337 if (set_laser_pose()) { 00338 laser_pose_set_ = true; 00339 apply_initial_pose(); 00340 } else { 00341 logger->log_warn(name(), "Could not determine laser pose, skipping loop"); 00342 return; 00343 } 00344 } 00345 00346 laser_if_->read(); 00347 if (! laser_if_->changed()) { 00348 //logger->log_warn(name(), "Laser data unchanged, skipping loop"); 00349 return; 00350 } 00351 float* laser_distances = laser_if_->distances(); 00352 00353 MutexLocker lock(conf_mutex_); 00354 00355 // Where was the robot when this scan was taken? 00356 tf::Stamped<tf::Pose> odom_pose; 00357 pf_vector_t pose; 00358 Time latest(0, 0); 00359 // cannot use laser_if_->timestamp() here, since odometry is updated in 00360 // last cycle of main loop while laser is newer -> tf extrapolation 00361 if (!get_odom_pose(odom_pose, pose.v[0], pose.v[1], pose.v[2], 00362 &latest, base_frame_id_)) 00363 { 00364 logger->log_error(name(), "Couldn't determine robot's pose " 00365 "associated with laser scan"); 00366 return; 00367 } 00368 00369 pf_vector_t delta = pf_vector_zero(); 00370 00371 if (pf_init_) { 00372 // Compute change in pose 00373 //delta = pf_vector_coord_sub(pose, pf_odom_pose_); 00374 delta.v[0] = pose.v[0] - pf_odom_pose_.v[0]; 00375 delta.v[1] = pose.v[1] - pf_odom_pose_.v[1]; 00376 delta.v[2] = angle_diff(pose.v[2], pf_odom_pose_.v[2]); 00377 00378 fawkes::Time now(clock); 00379 00380 // See if we should update the filter 00381 bool update = 00382 fabs(delta.v[0]) > d_thresh_ || 00383 fabs(delta.v[1]) > d_thresh_ || 00384 fabs(delta.v[2]) > a_thresh_; 00385 00386 if (update) { 00387 last_move_time_->stamp(); 00388 /* 00389 logger->log_info(name(), "(%f,%f,%f) vs. (%f,%f,%f) diff (%f|%i,%f|%i,%f|%i)", 00390 pose.v[0], pose.v[1], pose.v[2], 00391 pf_odom_pose_.v[0], pf_odom_pose_.v[1], pf_odom_pose_.v[2], 00392 fabs(delta.v[0]), fabs(delta.v[0]) > d_thresh_, 00393 fabs(delta.v[1]), fabs(delta.v[1]) > d_thresh_, 00394 fabs(delta.v[2]), fabs(delta.v[2]) > a_thresh_); 00395 */ 00396 00397 laser_update_ = true; 00398 } else if ((now - last_move_time_) <= t_thresh_) { 00399 laser_update_ = true; 00400 } 00401 } 00402 00403 bool force_publication = false; 00404 if (!pf_init_) { 00405 //logger->log_debug(name(), "! PF init"); 00406 // Pose at last filter update 00407 pf_odom_pose_ = pose; 00408 00409 // Filter is now initialized 00410 pf_init_ = true; 00411 00412 // Should update sensor data 00413 laser_update_ = true; 00414 force_publication = true; 00415 00416 resample_count_ = 0; 00417 } else if (pf_init_ && laser_update_) { 00418 //logger->log_debug(name(), "PF init && laser update"); 00419 //printf("pose\n"); 00420 //pf_vector_fprintf(pose, stdout, "%.3f"); 00421 00422 ::amcl::AMCLOdomData odata; 00423 odata.pose = pose; 00424 // HACK 00425 // Modify the delta in the action data so the filter gets 00426 // updated correctly 00427 odata.delta = delta; 00428 00429 // Use the action data to update the filter 00430 //logger->log_debug(name(), "Updating Odometry"); 00431 odom_->UpdateAction(pf_, (::amcl::AMCLSensorData*) &odata); 00432 00433 // Pose at last filter update 00434 //this->pf_odom_pose = pose; 00435 } 00436 00437 bool resampled = false; 00438 // If the robot has moved, update the filter 00439 if (laser_update_) { 00440 //logger->log_warn(name(), "laser update"); 00441 00442 ::amcl::AMCLLaserData ldata; 00443 ldata.sensor = laser_; 00444 ldata.range_count = angle_range_ + 1; 00445 00446 // To account for lasers that are mounted upside-down, we determine the 00447 // min, max, and increment angles of the laser in the base frame. 00448 // 00449 // Construct min and max angles of laser, in the base_link frame. 00450 Time latest(0, 0); 00451 tf::Quaternion q; 00452 q.setEulerZYX(angle_min_, 0.0, 0.0); 00453 tf::Stamped<tf::Quaternion> min_q(q, latest, laser_frame_id_); 00454 q.setEulerZYX(angle_min_ + angle_increment_, 0.0, 0.0); 00455 tf::Stamped<tf::Quaternion> inc_q(q, latest, laser_frame_id_); 00456 try 00457 { 00458 tf_listener->transform_quaternion(base_frame_id_, min_q, min_q); 00459 tf_listener->transform_quaternion(base_frame_id_, inc_q, inc_q); 00460 } 00461 catch(Exception &e) 00462 { 00463 logger->log_warn(name(), "Unable to transform min/max laser angles " 00464 "into base frame"); 00465 logger->log_warn(name(), e); 00466 return; 00467 } 00468 00469 double angle_min = tf::get_yaw(min_q); 00470 double angle_increment = tf::get_yaw(inc_q) - angle_min; 00471 00472 // wrapping angle to [-pi .. pi] 00473 angle_increment = fmod(angle_increment + 5 * M_PI, 2 * M_PI) - M_PI; 00474 00475 // Apply range min/max thresholds, if the user supplied them 00476 if (laser_max_range_ > 0.0) 00477 ldata.range_max = (float) laser_max_range_; 00478 else 00479 ldata.range_max = HUGE; 00480 double range_min; 00481 if (laser_min_range_ > 0.0) 00482 range_min = (float) laser_min_range_; 00483 else 00484 range_min = 0.0; 00485 // The AMCLLaserData destructor will free this memory 00486 ldata.ranges = new double[ldata.range_count][2]; 00487 00488 const unsigned int maxlen_dist = laser_if_->maxlenof_distances(); 00489 for (int i = 0; i < ldata.range_count; ++i) { 00490 unsigned int idx = (angle_min_idx_ + i) % maxlen_dist; 00491 // amcl doesn't (yet) have a concept of min range. So we'll map short 00492 // readings to max range. 00493 if (laser_distances[idx] <= range_min) 00494 ldata.ranges[i][0] = ldata.range_max; 00495 else 00496 ldata.ranges[i][0] = laser_distances[idx]; 00497 00498 // Compute bearing 00499 ldata.ranges[i][1] = angle_min + (idx * angle_increment); 00500 } 00501 00502 try { 00503 laser_->UpdateSensor(pf_, (::amcl::AMCLSensorData*) &ldata); 00504 } catch (Exception &e) { 00505 logger->log_warn(name(), "Failed to update laser sensor data, " 00506 "exception follows"); 00507 logger->log_warn(name(), e); 00508 } 00509 00510 laser_update_ = false; 00511 00512 pf_odom_pose_ = pose; 00513 00514 // Resample the particles 00515 if (!(++resample_count_ % resample_interval_)) { 00516 //logger->log_info(name(), "Resample!"); 00517 pf_update_resample(pf_); 00518 resampled = true; 00519 } 00520 00521 #ifdef HAVE_ROS 00522 pf_sample_set_t* set = (pf_->sets) + pf_->current_set; 00523 logger->log_debug(name(), "Num samples: %d", set->sample_count); 00524 00525 // Publish the resulting cloud 00526 // TODO: set maximum rate for publishing 00527 geometry_msgs::PoseArray cloud_msg; 00528 cloud_msg.header.stamp = ros::Time::now(); 00529 cloud_msg.header.frame_id = global_frame_id_; 00530 cloud_msg.poses.resize(set->sample_count); 00531 for (int i = 0; i < set->sample_count; i++) { 00532 tf::Quaternion q(tf::create_quaternion_from_yaw(set->samples[i].pose.v[2])); 00533 cloud_msg.poses[i].position.x = set->samples[i].pose.v[0]; 00534 cloud_msg.poses[i].position.y = set->samples[i].pose.v[1]; 00535 cloud_msg.poses[i].position.z = 0.; 00536 cloud_msg.poses[i].orientation.x = q.x(); 00537 cloud_msg.poses[i].orientation.y = q.y(); 00538 cloud_msg.poses[i].orientation.z = q.z(); 00539 cloud_msg.poses[i].orientation.w = q.w(); 00540 } 00541 00542 particlecloud_pub_.publish(cloud_msg); 00543 #endif 00544 } 00545 00546 if (resampled || force_publication) { 00547 // Read out the current hypotheses 00548 double max_weight = 0.0; 00549 int max_weight_hyp = -1; 00550 std::vector<amcl_hyp_t> hyps; 00551 hyps.resize(pf_->sets[pf_->current_set].cluster_count); 00552 for (int hyp_count = 0; 00553 hyp_count < pf_->sets[pf_->current_set].cluster_count; 00554 hyp_count++) { 00555 double weight; 00556 pf_vector_t pose_mean; 00557 pf_matrix_t pose_cov; 00558 if (!pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean, 00559 &pose_cov)) { 00560 logger->log_error(name(), "Couldn't get stats on cluster %d", 00561 hyp_count); 00562 break; 00563 } 00564 00565 hyps[hyp_count].weight = weight; 00566 hyps[hyp_count].pf_pose_mean = pose_mean; 00567 hyps[hyp_count].pf_pose_cov = pose_cov; 00568 00569 if (hyps[hyp_count].weight > max_weight) { 00570 max_weight = hyps[hyp_count].weight; 00571 max_weight_hyp = hyp_count; 00572 } 00573 } 00574 00575 if (max_weight > 0.0) { 00576 /* 00577 logger->log_debug(name(), "Max weight pose: %.3f %.3f %.3f (weight: %f)", 00578 hyps[max_weight_hyp].pf_pose_mean.v[0], 00579 hyps[max_weight_hyp].pf_pose_mean.v[1], 00580 hyps[max_weight_hyp].pf_pose_mean.v[2], max_weight); 00581 00582 puts(""); 00583 pf_matrix_fprintf(hyps[max_weight_hyp].pf_pose_cov, stdout, "%6.3f"); 00584 puts(""); 00585 */ 00586 #ifdef HAVE_ROS 00587 geometry_msgs::PoseWithCovarianceStamped p; 00588 // Fill in the header 00589 p.header.frame_id = global_frame_id_; 00590 p.header.stamp = ros::Time(); 00591 // Copy in the pose 00592 p.pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0]; 00593 p.pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1]; 00594 tf::Quaternion q(tf::Vector3(0,0,1), 00595 hyps[max_weight_hyp].pf_pose_mean.v[2]); 00596 p.pose.pose.orientation.x = q.x(); 00597 p.pose.pose.orientation.y = q.y(); 00598 p.pose.pose.orientation.z = q.z(); 00599 p.pose.pose.orientation.w = q.w(); 00600 // Copy in the covariance, converting from 3-D to 6-D 00601 pf_sample_set_t* set = pf_->sets + pf_->current_set; 00602 for (int i = 0; i < 2; i++) { 00603 for (int j = 0; j < 2; j++) { 00604 // Report the overall filter covariance, rather than the 00605 // covariance for the highest-weight cluster 00606 //p.covariance[6*i+j] = hyps[max_weight_hyp].pf_pose_cov.m[i][j]; 00607 last_covariance_[6 * i + j] = set->cov.m[i][j]; 00608 } 00609 } 00610 00611 // Report the overall filter covariance, rather than the 00612 // covariance for the highest-weight cluster 00613 //p.covariance[6*5+5] = hyps[max_weight_hyp].pf_pose_cov.m[2][2]; 00614 last_covariance_[6 * 5 + 5] = set->cov.m[2][2]; 00615 00616 pose_pub_.publish(p); 00617 #endif 00618 //last_published_pose = p; 00619 /* 00620 logger->log_debug(name(), "New pose: %6.3f %6.3f %6.3f", 00621 hyps[max_weight_hyp].pf_pose_mean.v[0], 00622 hyps[max_weight_hyp].pf_pose_mean.v[1], 00623 hyps[max_weight_hyp].pf_pose_mean.v[2]); 00624 */ 00625 00626 // subtracting base to odom from map to base and send map to odom instead 00627 tf::Stamped<tf::Pose> odom_to_map; 00628 00629 try { 00630 tf::Transform 00631 tmp_tf(tf::create_quaternion_from_yaw(hyps[max_weight_hyp].pf_pose_mean.v[2]), 00632 tf::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0], 00633 hyps[max_weight_hyp].pf_pose_mean.v[1], 0.0)); 00634 Time latest(0, 0); 00635 tf::Stamped<tf::Pose> tmp_tf_stamped(tmp_tf.inverse(), 00636 latest, base_frame_id_); 00637 tf_listener->transform_pose(odom_frame_id_, 00638 tmp_tf_stamped, odom_to_map); 00639 } catch (Exception &e) { 00640 logger->log_warn(name(), 00641 "Failed to subtract base to odom transform"); 00642 return; 00643 } 00644 00645 latest_tf_ = 00646 tf::Transform(tf::Quaternion(odom_to_map.getRotation()), 00647 tf::Point(odom_to_map.getOrigin())); 00648 latest_tf_valid_ = true; 00649 00650 // We want to send a transform that is good up until a 00651 // tolerance time so that odom can be used 00652 Time transform_expiration = 00653 (*laser_if_->timestamp() + transform_tolerance_); 00654 tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(), 00655 transform_expiration, 00656 global_frame_id_, odom_frame_id_); 00657 tf_publisher->send_transform(tmp_tf_stamped); 00658 00659 00660 // We need to apply the last transform to the latest odom pose to get 00661 // the latest map pose to store. We'll take the covariance from 00662 // last_published_pose. 00663 tf::Pose map_pose = latest_tf_.inverse() * odom_pose; 00664 tf::Quaternion map_att = map_pose.getRotation(); 00665 00666 double trans[3] = {map_pose.getOrigin().x(), map_pose.getOrigin().y(), 0}; 00667 double rot[4] = { map_att.x(), map_att.y(), map_att.z(), map_att.w() }; 00668 00669 if (pos3d_if_->visibility_history() >= 0) { 00670 pos3d_if_->set_visibility_history(pos3d_if_->visibility_history() + 1); 00671 } else { 00672 pos3d_if_->set_visibility_history(1); 00673 } 00674 pos3d_if_->set_translation(trans); 00675 pos3d_if_->set_rotation(rot); 00676 pos3d_if_->write(); 00677 00678 sent_first_transform_ = true; 00679 } else { 00680 logger->log_error(name(), "No pose!"); 00681 } 00682 } else if (latest_tf_valid_) { 00683 // Nothing changed, so we'll just republish the last transform, to keep 00684 // everybody happy. 00685 Time transform_expiration = 00686 (*laser_if_->timestamp() + transform_tolerance_); 00687 tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(), 00688 transform_expiration, 00689 global_frame_id_, odom_frame_id_); 00690 tf_publisher->send_transform(tmp_tf_stamped); 00691 00692 // Is it time to save our last pose to the param server 00693 Time now(clock); 00694 // We need to apply the last transform to the latest odom pose to get 00695 // the latest map pose to store. We'll take the covariance from 00696 // last_published_pose. 00697 tf::Pose map_pose = latest_tf_.inverse() * odom_pose; 00698 tf::Quaternion map_att = map_pose.getRotation(); 00699 00700 double trans[3] = {map_pose.getOrigin().x(), map_pose.getOrigin().y(), 0}; 00701 double rot[4] = { map_att.x(), map_att.y(), map_att.z(), map_att.w() }; 00702 00703 if (pos3d_if_->visibility_history() >= 0) { 00704 pos3d_if_->set_visibility_history(pos3d_if_->visibility_history() + 1); 00705 } else { 00706 pos3d_if_->set_visibility_history(1); 00707 } 00708 pos3d_if_->set_translation(trans); 00709 pos3d_if_->set_rotation(rot); 00710 pos3d_if_->write(); 00711 00712 /* 00713 if ((save_pose_period > 0.0) && 00714 (now - save_pose_last_time) >= save_pose_period) { 00715 double yaw, pitch, roll; 00716 map_pose.getBasis().getEulerYPR(yaw, pitch, roll); 00717 private_nh_.setParam("initial_pose_x", map_pose.getOrigin().x()); 00718 private_nh_.setParam("initial_pose_y", map_pose.getOrigin().y()); 00719 private_nh_.setParam("initial_pose_a", yaw); 00720 private_nh_.setParam("initial_cov_xx", 00721 last_published_pose.pose.covariance[6 * 0 + 0]); 00722 private_nh_.setParam("initial_cov_yy", 00723 last_published_pose.pose.covariance[6 * 1 + 1]); 00724 private_nh_.setParam("initial_cov_aa", 00725 last_published_pose.pose.covariance[6 * 5 + 5]); 00726 save_pose_last_time = now; 00727 } 00728 */ 00729 } else { 00730 if (pos3d_if_->visibility_history() <= 0) { 00731 pos3d_if_->set_visibility_history(pos3d_if_->visibility_history() - 1); 00732 } else { 00733 pos3d_if_->set_visibility_history(-1); 00734 } 00735 pos3d_if_->write(); 00736 } 00737 } 00738 00739 void AmclThread::finalize() 00740 { 00741 if (map_) { 00742 map_free(map_); 00743 map_ = NULL; 00744 } 00745 delete initial_pose_hyp_; 00746 initial_pose_hyp_ = NULL; 00747 00748 delete last_move_time_; 00749 00750 blackboard->close(laser_if_); 00751 blackboard->close(pos3d_if_); 00752 00753 #ifdef HAVE_ROS 00754 pose_pub_.shutdown(); 00755 particlecloud_pub_.shutdown(); 00756 initial_pose_sub_.shutdown(); 00757 map_pub_.shutdown(); 00758 #endif 00759 } 00760 00761 bool 00762 AmclThread::get_odom_pose(tf::Stamped<tf::Pose>& odom_pose, double& x, 00763 double& y, double& yaw, 00764 const fawkes::Time* t, const std::string& f) 00765 { 00766 // Get the robot's pose 00767 tf::Stamped<tf::Pose> 00768 ident(tf::Transform(tf::Quaternion(0, 0, 0, 1), 00769 tf::Vector3(0, 0, 0)), t, f); 00770 try { 00771 tf_listener->transform_pose(odom_frame_id_, ident, odom_pose); 00772 } catch (Exception &e) { 00773 logger->log_warn(name(), 00774 "Failed to compute odom pose, skipping scan (%s)", e.what()); 00775 return false; 00776 } 00777 x = odom_pose.getOrigin().x(); 00778 y = odom_pose.getOrigin().y(); 00779 double pitch, roll; 00780 odom_pose.getBasis().getEulerZYX(yaw, pitch, roll); 00781 00782 //logger->log_info(name(), "Odom pose: (%f, %f, %f)", x, y, yaw); 00783 00784 return true; 00785 } 00786 00787 00788 #ifdef HAVE_ROS 00789 # ifdef USE_MAP_PUB 00790 void 00791 AmclThread::publish_map() 00792 { 00793 nav_msgs::OccupancyGrid msg; 00794 msg.info.map_load_time = ros::Time::now(); 00795 msg.header.stamp = ros::Time::now(); 00796 msg.header.frame_id = "map"; 00797 00798 msg.info.width = map_width_; 00799 msg.info.height = map_height_; 00800 msg.info.resolution = cfg_resolution_; 00801 msg.info.origin.position.x = 0.; //map_->origin_x; 00802 msg.info.origin.position.y = 0.; //map_->origin_y; 00803 msg.info.origin.position.z = 0.0; 00804 tf::Quaternion q(tf::create_quaternion_from_yaw(0.)); 00805 msg.info.origin.orientation.x = q.x(); 00806 msg.info.origin.orientation.y = q.y(); 00807 msg.info.origin.orientation.z = q.z(); 00808 msg.info.origin.orientation.w = q.w(); 00809 00810 // Allocate space to hold the data 00811 msg.data.resize(msg.info.width * msg.info.height); 00812 00813 for (unsigned int i = 0; i < msg.info.width * msg.info.height; ++i) { 00814 if (map_->cells[i].occ_state == +1) { 00815 msg.data[i] = +100; 00816 } else if (map_->cells[i].occ_state == -1) { 00817 msg.data[i] = 0; 00818 } else { 00819 msg.data[i] = -1; 00820 } 00821 } 00822 00823 map_pub_.publish(msg); 00824 } 00825 # endif 00826 #endif 00827 00828 00829 bool 00830 AmclThread::set_laser_pose() 00831 { 00832 //logger->log_debug(name(), "Transform 1"); 00833 fawkes::Time now(clock); 00834 tf::Stamped<tf::Pose> 00835 ident(tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0, 0, 0)), 00836 &now, laser_frame_id_); 00837 tf::Stamped<tf::Pose> laser_pose; 00838 try { 00839 tf_listener->transform_pose(base_frame_id_, ident, laser_pose); 00840 } catch (fawkes::tf::LookupException& e) { 00841 //logger->log_error(name(), "Failed to lookup transform from %s to %s.", 00842 // laser_frame_id_.c_str(), base_frame_id_.c_str()); 00843 //logger->log_error(name(), e); 00844 return false; 00845 } catch (fawkes::tf::TransformException& e) { 00846 //logger->log_error(name(), "Transform error from %s to %s, exception follows.", 00847 // laser_frame_id_.c_str(), base_frame_id_.c_str()); 00848 //logger->log_error(name(), e); 00849 return false; 00850 } catch (fawkes::Exception& e) { 00851 logger->log_error(name(), "Generic exception for transform from %s to %s.", 00852 laser_frame_id_.c_str(), base_frame_id_.c_str()); 00853 logger->log_error(name(), e); 00854 return false; 00855 } 00856 00857 /* 00858 tf::Stamped<tf::Pose> 00859 ident(tf::Transform(tf::Quaternion(0, 0, 0, 1), 00860 tf::Vector3(0, 0, 0)), Time(), laser_frame_id_); 00861 tf::Stamped<tf::Pose> laser_pose; 00862 00863 try { 00864 tf_listener->transform_pose(base_frame_id_, ident, laser_pose); 00865 } catch (tf::TransformException& e) { 00866 logger->log_error(name(), "Couldn't transform from %s to %s, " 00867 "even though the message notifier is in use", 00868 laser_frame_id_.c_str(), base_frame_id_.c_str()); 00869 logger->log_error(name(), e); 00870 return; 00871 } 00872 */ 00873 00874 pf_vector_t laser_pose_v; 00875 laser_pose_v.v[0] = laser_pose.getOrigin().x(); 00876 laser_pose_v.v[1] = laser_pose.getOrigin().y(); 00877 00878 // laser mounting angle gets computed later -> set to 0 here! 00879 laser_pose_v.v[2] = tf::get_yaw(laser_pose.getRotation()); 00880 laser_->SetLaserPose(laser_pose_v); 00881 logger->log_debug(name(), 00882 "Received laser's pose wrt robot: %.3f %.3f %.3f", 00883 laser_pose_v.v[0], laser_pose_v.v[1], laser_pose_v.v[2]); 00884 00885 return true; 00886 } 00887 00888 void 00889 AmclThread::apply_initial_pose() 00890 { 00891 if (initial_pose_hyp_ != NULL && map_ != NULL) { 00892 logger->log_info(name(), "Applying pose: %.3f %.3f %.3f " 00893 "(cov: %.3f %.3f %.3f, %.3f %.3f %.3f, %.3f %.3f %.3f)", 00894 initial_pose_hyp_->pf_pose_mean.v[0], 00895 initial_pose_hyp_->pf_pose_mean.v[1], 00896 initial_pose_hyp_->pf_pose_mean.v[2], 00897 initial_pose_hyp_->pf_pose_cov.m[0][0], 00898 initial_pose_hyp_->pf_pose_cov.m[0][1], 00899 initial_pose_hyp_->pf_pose_cov.m[0][2], 00900 initial_pose_hyp_->pf_pose_cov.m[1][0], 00901 initial_pose_hyp_->pf_pose_cov.m[1][1], 00902 initial_pose_hyp_->pf_pose_cov.m[1][2], 00903 initial_pose_hyp_->pf_pose_cov.m[2][0], 00904 initial_pose_hyp_->pf_pose_cov.m[2][1], 00905 initial_pose_hyp_->pf_pose_cov.m[2][2]); 00906 pf_init(pf_, initial_pose_hyp_->pf_pose_mean, 00907 initial_pose_hyp_->pf_pose_cov); 00908 pf_init_ = false; 00909 } else { 00910 logger->log_warn(name(), "Called apply initial pose but no pose to apply"); 00911 } 00912 } 00913 00914 pf_vector_t 00915 AmclThread::uniform_pose_generator(void* arg) 00916 { 00917 map_t* map = (map_t*) arg; 00918 #if NEW_UNIFORM_SAMPLING 00919 unsigned int rand_index = drand48() * free_space_indices.size(); 00920 std::pair<int,int> free_point = free_space_indices[rand_index]; 00921 pf_vector_t p; 00922 p.v[0] = MAP_WXGX(map, free_point.first); 00923 p.v[1] = MAP_WYGY(map, free_point.second); 00924 p.v[2] = drand48() * 2 * M_PI - M_PI; 00925 #else 00926 double min_x, max_x, min_y, max_y; 00927 min_x = (map->size_x * map->scale) / 2.0 - map->origin_x; 00928 max_x = (map->size_x * map->scale) / 2.0 + map->origin_x; 00929 min_y = (map->size_y * map->scale) / 2.0 - map->origin_y; 00930 max_y = (map->size_y * map->scale) / 2.0 + map->origin_y; 00931 00932 pf_vector_t p; 00933 for (;;) { 00934 p.v[0] = min_x + drand48() * (max_x - min_x); 00935 p.v[1] = min_y + drand48() * (max_y - min_y); 00936 p.v[2] = drand48() * 2 * M_PI - M_PI; 00937 // Check that it's a free cell 00938 int i, j; 00939 i = MAP_GXWX(map, p.v[0]); 00940 j = MAP_GYWY(map, p.v[1]); 00941 if (MAP_VALID(map,i,j) && (map->cells[MAP_INDEX(map,i,j)].occ_state == -1)) 00942 break; 00943 } 00944 #endif 00945 return p; 00946 } 00947 00948 00949 #ifdef HAVE_ROS 00950 void 00951 AmclThread::initial_pose_received(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg) 00952 { 00953 MutexLocker lock(conf_mutex_); 00954 if(msg->header.frame_id == "") { 00955 // This should be removed at some point 00956 logger->log_warn(name(), "Received initial pose with empty frame_id. " 00957 "You should always supply a frame_id."); 00958 } else if (tf_listener->resolve(msg->header.frame_id) != tf_listener->resolve(global_frame_id_)) 00959 { 00960 // We only accept initial pose estimates in the global frame, #5148. 00961 logger->log_warn(name(),"Ignoring initial pose in frame \"%s\"; " 00962 "initial poses must be in the global frame, \"%s\"", 00963 msg->header.frame_id.c_str(), 00964 global_frame_id_.c_str()); 00965 return; 00966 } 00967 00968 fawkes::Time latest(0, 0); 00969 fawkes::Time msg_time(msg->header.stamp.sec, 00970 msg->header.stamp.nsec / 1000); 00971 00972 // In case the client sent us a pose estimate in the past, integrate the 00973 // intervening odometric change. 00974 tf::StampedTransform tx_odom; 00975 try { 00976 tf_listener->lookup_transform(base_frame_id_, latest, 00977 base_frame_id_, msg_time, 00978 global_frame_id_, tx_odom); 00979 } catch(tf::TransformException &e) { 00980 // If we've never sent a transform, then this is normal, because the 00981 // global_frame_id_ frame doesn't exist. We only care about in-time 00982 // transformation for on-the-move pose-setting, so ignoring this 00983 // startup condition doesn't really cost us anything. 00984 if(sent_first_transform_) 00985 logger->log_warn(name(), "Failed to transform initial pose " 00986 "in time (%s)", e.what()); 00987 tx_odom.setIdentity(); 00988 } catch (Exception &e) { 00989 logger->log_warn(name(), "Failed to transform initial pose in time"); 00990 logger->log_warn(name(), e); 00991 } 00992 00993 tf::Pose pose_old, pose_new; 00994 pose_old = 00995 tf::Transform(tf::Quaternion(msg->pose.pose.orientation.x, 00996 msg->pose.pose.orientation.y, 00997 msg->pose.pose.orientation.z, 00998 msg->pose.pose.orientation.w), 00999 tf::Vector3(msg->pose.pose.position.x, 01000 msg->pose.pose.position.y, 01001 msg->pose.pose.position.z)); 01002 pose_new = tx_odom.inverse() * pose_old; 01003 01004 // Transform into the global frame 01005 01006 logger->log_info(name(), "Setting pose: %.3f %.3f %.3f", 01007 pose_new.getOrigin().x(), 01008 pose_new.getOrigin().y(), 01009 tf::get_yaw(pose_new)); 01010 // Re-initialize the filter 01011 pf_vector_t pf_init_pose_mean = pf_vector_zero(); 01012 pf_init_pose_mean.v[0] = pose_new.getOrigin().x(); 01013 pf_init_pose_mean.v[1] = pose_new.getOrigin().y(); 01014 pf_init_pose_mean.v[2] = tf::get_yaw(pose_new); 01015 pf_matrix_t pf_init_pose_cov = pf_matrix_zero(); 01016 // Copy in the covariance, converting from 6-D to 3-D 01017 for(int i=0; i<2; i++) { 01018 for(int j=0; j<2; j++) { 01019 pf_init_pose_cov.m[i][j] = msg->pose.covariance[6*i+j]; 01020 } 01021 } 01022 pf_init_pose_cov.m[2][2] = msg->pose.covariance[6*5+5]; 01023 01024 delete initial_pose_hyp_; 01025 initial_pose_hyp_ = new amcl_hyp_t(); 01026 initial_pose_hyp_->pf_pose_mean = pf_init_pose_mean; 01027 initial_pose_hyp_->pf_pose_cov = pf_init_pose_cov; 01028 apply_initial_pose(); 01029 01030 last_move_time_->stamp(); 01031 } 01032 #endif