Fawkes API  Fawkes Development Version
amcl_thread.cpp
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