gmcl_node.cpp

Go to the documentation of this file.

//this package is based on amcl and has been modified to fit gmcl 
/* 
 * Author: Mhd Ali Alshikh Khalil
 * Date: 20 June 2021
 * 
*/
 
//amcl author clarification
/*
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License as published by the Free Software Foundation; either
 *  version 2.1 of the License, or (at your option) any later version.
 *
 *  This library is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *  Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */
 
/* Author: Brian Gerkey */
 
#include <algorithm>
#include <vector>
#include <map>
#include <cmath>
#include <memory>
 
#include <boost/bind.hpp>
#include <boost/thread/mutex.hpp>
 
// Signal handling
#include <signal.h>
 
#include "gmcl/map/map.h"
#include "gmcl/pf/pf.h"
#include "gmcl/sensors/gmcl_odom.h"
#include "gmcl/sensors/gmcl_laser.h"
#include "portable_utils.hpp"
 
#include "ros/assert.h"
 
// roscpp
#include "ros/ros.h"
 
// Messages that I need
#include "sensor_msgs/LaserScan.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "geometry_msgs/PoseArray.h"
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/PoseStamped.h"
#include "nav_msgs/GetMap.h"
#include "nav_msgs/SetMap.h"
#include "std_srvs/Empty.h"
 
// For transform support
#include "tf2/LinearMath/Transform.h"
#include "tf2/convert.h"
#include "tf2/utils.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/message_filter.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_ros/transform_listener.h"
#include "message_filters/subscriber.h"
 
// Dynamic_reconfigure
#include "dynamic_reconfigure/server.h"
#include "gmcl/GMCLConfig.h"
 
// Allows AMCL to run from bag file
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <boost/foreach.hpp>
 
// For monitoring the estimator
#include <diagnostic_updater/diagnostic_updater.h>
 
#define NEW_UNIFORM_SAMPLING 1
 
using namespace gmcl;
 
// Pose hypothesis
typedef struct
{
  // Total weight (weights sum to 1)
  double weight;
 
  // Mean of pose esimate
  pf_vector_t pf_pose_mean;
 
  // Covariance of pose estimate
  pf_matrix_t pf_pose_cov;
 
} gmcl_hyp_t;
 
static double
normalize(double z)
{
  return atan2(sin(z),cos(z));
}
static double
angle_diff(double a, double b)
{
  double d1, d2;
  a = normalize(a);
  b = normalize(b);
  d1 = a-b;
  d2 = 2*M_PI - fabs(d1);
  if(d1 > 0)
    d2 *= -1.0;
  if(fabs(d1) < fabs(d2))
    return(d1);
  else
    return(d2);
}
 
static const std::string scan_topic_ = "scan" ;
 
/* This function is only useful to have the whole code work
 * with old rosbags that have trailing slashes for their frames
 */
inline
std::string stripSlash(const std::string& in)
{
  std::string out = in;
  if ( ( !in.empty() ) && (in[0] == '/') )
    out.erase(0,1);
  return out;
}
 
class GmclNode
{
  public:
    GmclNode();
    ~GmclNode();
 
    void runFromBag(const std::string &in_bag_fn, bool trigger_global_localization = false);
 
    int process();
    void savePoseToServer();
 
  private:
    std::shared_ptr<tf2_ros::TransformBroadcaster> tfb_;
    std::shared_ptr<tf2_ros::TransformListener> tfl_;
    std::shared_ptr<tf2_ros::Buffer> tf_;
 
    bool sent_first_transform_;
 
    tf2::Transform latest_tf_;
    bool latest_tf_valid_;
 
    // Pose-generating function used to uniformly distribute particles over
    // the map
    static pf_vector_t uniformPoseGenerator(pf_t *pf, void* arg , void* e_arg );
#if NEW_UNIFORM_SAMPLING
    static std::vector<std::pair<int,int> > free_space_indices;
#endif
    // Callbacks
    bool globalLocalizationCallback(std_srvs::Empty::Request& req,
                                    std_srvs::Empty::Response& res);
    bool nomotionUpdateCallback(std_srvs::Empty::Request& req,
                                    std_srvs::Empty::Response& res);
    bool setMapCallback(nav_msgs::SetMap::Request& req,
                        nav_msgs::SetMap::Response& res);
  
    
    void handelNewLaser(const sensor_msgs::LaserScanConstPtr& laser_scan);
    void laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan);
    void initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg);
    void handleInitialPoseMessage(const geometry_msgs::PoseWithCovarianceStamped& msg);
    void mapReceived(const nav_msgs::OccupancyGridConstPtr& msg);
 
    void handleMapMessage(const nav_msgs::OccupancyGrid& msg);
    void freeMapDependentMemory();
    map_t* convertMap( const nav_msgs::OccupancyGrid& map_msg );
    void updatePoseFromServer();
    void applyInitialPose();
 
    //parameter for what odom to use
    std::string odom_frame_id_;
 
    //paramater to store latest odom pose
    geometry_msgs::PoseStamped latest_odom_pose_;
 
    //parameter for what base to use
    std::string base_frame_id_;
    std::string global_frame_id_;
 
    bool use_map_topic_;
    bool first_map_only_;
 
    ros::Duration gui_publish_period;
    ros::Time save_pose_last_time;
    ros::Duration save_pose_period;
 
    geometry_msgs::PoseWithCovarianceStamped last_published_pose;
 
    map_t* map_;
    char* mapdata;
    int sx, sy;
    double resolution;
    
    energy_map_t* energy_map_;
    double resolution_x_ , resolution_y_;
    double threshold_val_;
    bool pub_energy_map_;
    
    message_filters::Subscriber<sensor_msgs::LaserScan>* laser_scan_sub_;
    tf2_ros::MessageFilter<sensor_msgs::LaserScan>* laser_scan_filter_;
    ros::Subscriber initial_pose_sub_;
    std::vector< GMCLLaser* > lasers_;
    std::vector< GMCLLaserData* > ldata_;
    std::vector< bool > lasers_update_;
    std::map< std::string, int > frame_to_laser_;
 
    // Particle filter
    pf_t *pf_;
    double pf_err_, pf_z_;
    bool pf_init_;
    pf_vector_t pf_odom_pose_;
    double d_thresh_, a_thresh_;
    int resample_interval_;
    int resample_count_;
    double laser_min_range_;
    double laser_max_range_; 
    pf_model_type pf_model_;
    
    //Nomotion update control
    bool m_force_update;  // used to temporarily let amcl update samples even when no motion occurs...
 
    GMCLOdom* odom_;
    GMCLLaser* laser_;
    int laser_index ;
    
    ros::Duration cloud_pub_interval;
    ros::Time last_cloud_pub_time;
 
    // For slowing play-back when reading directly from a bag file
    ros::WallDuration bag_scan_period_;
 
    void requestMap();
 
    // Helper to get odometric pose from transform system
    bool getOdomPose(geometry_msgs::PoseStamped& pose,
                     double& x, double& y, double& yaw,
                     const ros::Time& t, const std::string& f);
 
    //time for tolerance on the published transform,
    //basically defines how long a map->odom transform is good for
    ros::Duration transform_tolerance_;
 
    ros::NodeHandle nh_;
    ros::NodeHandle private_nh_;
    ros::Publisher pose_pub_;
    ros::Publisher particlecloud_pub_;
    ros::Publisher energy_particlecloud_pub_;
    ros::ServiceServer global_loc_srv_;
    ros::ServiceServer nomotion_update_srv_; //to let amcl update samples without requiring motion
    ros::ServiceServer set_map_srv_;
    ros::Subscriber initial_pose_sub_old_;
    ros::Subscriber map_sub_;
 
    diagnostic_updater::Updater diagnosic_updater_;
    void standardDeviationDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& diagnostic_status);
    double std_warn_level_x_;
    double std_warn_level_y_;
    double std_warn_level_yaw_;
 
    gmcl_hyp_t* initial_pose_hyp_;
    bool first_map_received_;
    bool first_reconfigure_call_;
      
    boost::recursive_mutex configuration_mutex_;
    dynamic_reconfigure::Server<gmcl::GMCLConfig> *dsrv_;
    gmcl::GMCLConfig default_config_;
    ros::Timer check_laser_timer_;
 
    int max_beams_, min_particles_, max_particles_ , N_aux_particles_;
    double alpha1_, alpha2_, alpha3_, alpha4_, alpha5_;
    double alpha_slow_, alpha_fast_;
    double crossover_alpha_, mutation_prob_;
    double z_hit_, z_short_, z_max_, z_rand_, sigma_hit_, lambda_short_;
  //beam skip related params
    bool do_beamskip_;
    double beam_skip_distance_, beam_skip_threshold_, beam_skip_error_threshold_;
    double laser_likelihood_max_dist_;
    odom_model_t odom_model_type_;
    double init_pose_[3];
    double init_cov_[3];
    laser_model_t laser_model_type_;
    bool tf_broadcast_;
    bool selective_resampling_;
 
 
    void reconfigureCB(gmcl::GMCLConfig &config, uint32_t level);
 
    ros::Time last_laser_received_ts_;
    ros::Duration laser_check_interval_;
    void checkLaserReceived(const ros::TimerEvent& event);
};
#if NEW_UNIFORM_SAMPLING
std::vector<std::pair<int,int> > GmclNode::free_space_indices;
#endif
 
#define USAGE "USAGE: gmcl"
 
boost::shared_ptr<GmclNode> gmcl_node_ptr;
 
void sigintHandler(int sig)
{
  // Save latest pose as we're shutting down.
  gmcl_node_ptr->savePoseToServer();
  ros::shutdown();
}
 
int
main(int argc, char** argv)
{
  ros::init(argc, argv, "gmcl");
  ros::NodeHandle nh;
 
  // Override default sigint handler
  signal(SIGINT, sigintHandler);
 
  // Make our node available to sigintHandler
  gmcl_node_ptr.reset(new GmclNode());
 
  if (argc == 1)
  {
    // run using ROS input
    ros::spin();
  }
  else if ((argc >= 3) && (std::string(argv[1]) == "--run-from-bag"))
  {
    if (argc == 3)
    {
      gmcl_node_ptr->runFromBag(argv[2]);
    }
    else if ((argc == 4) && (std::string(argv[3]) == "--global-localization"))
    {
      gmcl_node_ptr->runFromBag(argv[2], true);
    }
  }
 
  // Without this, our boost locks are not shut down nicely
  gmcl_node_ptr.reset();
 
  // To quote Morgan, Hooray!
  return(0);
}
 
GmclNode::GmclNode() :
        sent_first_transform_(false),
        latest_tf_valid_(false),
        map_(NULL),
        pf_(NULL),
        resample_count_(0),
        odom_(NULL),
        laser_(NULL),
              private_nh_("~"),
        initial_pose_hyp_(NULL),
        first_map_received_(false),
        first_reconfigure_call_(true)
{
  boost::recursive_mutex::scoped_lock l(configuration_mutex_);
 
  // Grab params off the param server
  private_nh_.param("use_optimal_filter",  pf_model_.use_optimal_filter, false);
  private_nh_.param("use_intelligent_filter", pf_model_.use_crossover_mutation, false);
  private_nh_.param("use_self_adaptive",  pf_model_.use_self_adaptive, false);
  private_nh_.param("use_kld_sampling", pf_model_.use_adaptive_sampling, true);
   
  private_nh_.param("use_map_topic", use_map_topic_, false);
  private_nh_.param("first_map_only", first_map_only_, false);
  
  private_nh_.param("energy_map_resolution_x", resolution_x_, 0.2);
  private_nh_.param("energy_map_resolution_y", resolution_y_, 0.2);
  if( !resolution_x_ || !resolution_y_)
     { ROS_ERROR("don't set either of energy map resolutions to zero"); 
       ros::shutdown();
     }
  private_nh_.param("energy_threshold_value", threshold_val_, 0.05);
  private_nh_.param("publish_ser", pub_energy_map_, false);
  
  double tmp;
  private_nh_.param("gui_publish_rate", tmp, -1.0);
  gui_publish_period = ros::Duration(1.0/tmp);
  private_nh_.param("save_pose_rate", tmp, 0.5);
  save_pose_period = ros::Duration(1.0/tmp);
 
  private_nh_.param("laser_min_range", laser_min_range_, -1.0);
  private_nh_.param("laser_max_range", laser_max_range_, -1.0);
  private_nh_.param("laser_max_beams", max_beams_, 30);
  private_nh_.param("min_particles", min_particles_, 100);
  private_nh_.param("max_particles", max_particles_, 5000);
  private_nh_.param("N_aux_particles", N_aux_particles_, 10); 
  private_nh_.param("kld_err", pf_err_, 0.01);
  private_nh_.param("kld_z", pf_z_, 0.99);
  private_nh_.param("odom_alpha1", alpha1_, 0.2);
  private_nh_.param("odom_alpha2", alpha2_, 0.2);
  private_nh_.param("odom_alpha3", alpha3_, 0.2);
  private_nh_.param("odom_alpha4", alpha4_, 0.2);
  private_nh_.param("odom_alpha5", alpha5_, 0.2);
  
  private_nh_.param("do_beamskip", do_beamskip_, false);
  private_nh_.param("beam_skip_distance", beam_skip_distance_, 0.5);
  private_nh_.param("beam_skip_threshold", beam_skip_threshold_, 0.3);
  if (private_nh_.hasParam("beam_skip_error_threshold_"))
  {
    private_nh_.param("beam_skip_error_threshold_", beam_skip_error_threshold_);
  }
  else
  {
    private_nh_.param("beam_skip_error_threshold", beam_skip_error_threshold_, 0.9);
  }
 
  private_nh_.param("laser_z_hit", z_hit_, 0.95);
  private_nh_.param("laser_z_short", z_short_, 0.1);
  private_nh_.param("laser_z_max", z_max_, 0.05);
  private_nh_.param("laser_z_rand", z_rand_, 0.05);
  private_nh_.param("laser_sigma_hit", sigma_hit_, 0.2);
  private_nh_.param("laser_lambda_short", lambda_short_, 0.1);
  private_nh_.param("laser_likelihood_max_dist", laser_likelihood_max_dist_, 2.0);
  std::string tmp_model_type;
  private_nh_.param("laser_model_type", tmp_model_type, std::string("likelihood_field"));
  if(tmp_model_type == "beam")
    laser_model_type_ = LASER_MODEL_BEAM;
  else if(tmp_model_type == "likelihood_field")
    laser_model_type_ = LASER_MODEL_LIKELIHOOD_FIELD;
  else if(tmp_model_type == "likelihood_field_prob"){
    laser_model_type_ = LASER_MODEL_LIKELIHOOD_FIELD_PROB;
  }
  else
  {
    ROS_WARN("Unknown laser model type \"%s\"; defaulting to likelihood_field model",
             tmp_model_type.c_str());
    laser_model_type_ = LASER_MODEL_LIKELIHOOD_FIELD;
  }
 
  private_nh_.param("odom_model_type", tmp_model_type, std::string("diff"));
  if(tmp_model_type == "diff")
    odom_model_type_ = ODOM_MODEL_DIFF;
  else if(tmp_model_type == "omni")
    odom_model_type_ = ODOM_MODEL_OMNI;
  else if(tmp_model_type == "diff-corrected")
    odom_model_type_ = ODOM_MODEL_DIFF_CORRECTED;
  else if(tmp_model_type == "omni-corrected")
    odom_model_type_ = ODOM_MODEL_OMNI_CORRECTED;
  else
  {
    ROS_WARN("Unknown odom model type \"%s\"; defaulting to diff model",
             tmp_model_type.c_str());
    odom_model_type_ = ODOM_MODEL_DIFF;
  }
 
  private_nh_.param("update_min_d", d_thresh_, 0.2);
  private_nh_.param("update_min_a", a_thresh_, M_PI/6.0);
  private_nh_.param("odom_frame_id", odom_frame_id_, std::string("odom"));
  private_nh_.param("base_frame_id", base_frame_id_, std::string("base_link"));
  private_nh_.param("global_frame_id", global_frame_id_, std::string("map"));
  private_nh_.param("resample_interval", resample_interval_, 2);
  private_nh_.param("selective_resampling", selective_resampling_, false);
  double tmp_tol;
  private_nh_.param("transform_tolerance", tmp_tol, 0.1);
  private_nh_.param("recovery_alpha_slow", alpha_slow_, 0.001);
  private_nh_.param("recovery_alpha_fast", alpha_fast_, 0.1); 
  private_nh_.param("mutation_probability", mutation_prob_, 0.1); 
  private_nh_.param("crossover_alpha", crossover_alpha_, 0.5); 
  private_nh_.param("tf_broadcast", tf_broadcast_, true);
 
  // For diagnostics
  private_nh_.param("std_warn_level_x", std_warn_level_x_, 0.2);
  private_nh_.param("std_warn_level_y", std_warn_level_y_, 0.2);
  private_nh_.param("std_warn_level_yaw", std_warn_level_yaw_, 0.1);
 
  transform_tolerance_.fromSec(tmp_tol);
 
  {
    double bag_scan_period;
    private_nh_.param("bag_scan_period", bag_scan_period, -1.0);
    bag_scan_period_.fromSec(bag_scan_period);
  }
 
  odom_frame_id_ = stripSlash(odom_frame_id_);
  base_frame_id_ = stripSlash(base_frame_id_);
  global_frame_id_ = stripSlash(global_frame_id_);
 
  updatePoseFromServer();
 
  cloud_pub_interval.fromSec(1.0);
  tfb_.reset(new tf2_ros::TransformBroadcaster());
  tf_.reset(new tf2_ros::Buffer());
  tfl_.reset(new tf2_ros::TransformListener(*tf_));
 
  pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("gmcl_pose", 2, true);
  particlecloud_pub_ = nh_.advertise<geometry_msgs::PoseArray>("gmcl_particlecloud", 2, true);
  energy_particlecloud_pub_ = nh_.advertise<geometry_msgs::PoseArray>("gmcl_SER", 2, true);
  global_loc_srv_ = nh_.advertiseService("global_localization", 
                                         &GmclNode::globalLocalizationCallback,
                                         this);
  nomotion_update_srv_= nh_.advertiseService("request_nomotion_update", &GmclNode::nomotionUpdateCallback, this);
  set_map_srv_= nh_.advertiseService("set_map", &GmclNode::setMapCallback, this);
 
  laser_scan_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(nh_, scan_topic_, 100);
  laser_scan_filter_ = 
          new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*laser_scan_sub_,
                                                             *tf_,
                                                             odom_frame_id_,
                                                             100,
                                                             nh_);
  laser_scan_filter_->registerCallback(boost::bind(&GmclNode::laserReceived,
                                                   this, _1));
  initial_pose_sub_ = nh_.subscribe("initialpose", 2, &GmclNode::initialPoseReceived, this);
 
  if(use_map_topic_) {
    map_sub_ = nh_.subscribe("map", 1, &GmclNode::mapReceived, this);
    ROS_INFO("Subscribed to map topic.");
  } else {
    requestMap();
  }
  m_force_update = false;
 
  dsrv_ = new dynamic_reconfigure::Server<gmcl::GMCLConfig>(ros::NodeHandle("~"));
  dynamic_reconfigure::Server<gmcl::GMCLConfig>::CallbackType cb = boost::bind(&GmclNode::reconfigureCB, this, _1, _2);
  dsrv_->setCallback(cb);
 
  // 15s timer to warn on lack of receipt of laser scans, #5209
  laser_check_interval_ = ros::Duration(15.0);
  check_laser_timer_ = nh_.createTimer(laser_check_interval_, 
                                       boost::bind(&GmclNode::checkLaserReceived, this, _1));
 
  diagnosic_updater_.setHardwareID("None");
  diagnosic_updater_.add("Standard deviation", this, &GmclNode::standardDeviationDiagnostics);
}
 
void GmclNode::reconfigureCB(gmcl::GMCLConfig &config, uint32_t level)
{
  boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);
 
  //we don't want to do anything on the first call
  //which corresponds to startup
  if(first_reconfigure_call_)
  {
    first_reconfigure_call_ = false;
    default_config_ = config;
    return;
  }
 
  if(config.restore_defaults) {
    config = default_config_;
    //avoid looping
    config.restore_defaults = false;
  }
 
  resolution_x_ = config.energy_map_resolution_x;
  resolution_y_ = config.energy_map_resolution_y;
  if( !resolution_x_ || !resolution_y_)
     { ROS_ERROR("don't set either of energy map resolutions to zero"); 
       ros::shutdown();
     }
  threshold_val_ = config.energy_threshold_value;
  pub_energy_map_ = config.publish_ser;
  
  d_thresh_ = config.update_min_d;
  a_thresh_ = config.update_min_a;
 
  resample_interval_ = config.resample_interval;
 
  laser_min_range_ = config.laser_min_range;
  laser_max_range_ = config.laser_max_range;
 
  gui_publish_period = ros::Duration(1.0/config.gui_publish_rate);
  save_pose_period = ros::Duration(1.0/config.save_pose_rate);
 
  transform_tolerance_.fromSec(config.transform_tolerance);
 
  max_beams_ = config.laser_max_beams;
  alpha1_ = config.odom_alpha1;
  alpha2_ = config.odom_alpha2;
  alpha3_ = config.odom_alpha3;
  alpha4_ = config.odom_alpha4;
  alpha5_ = config.odom_alpha5;
 
  z_hit_ = config.laser_z_hit;
  z_short_ = config.laser_z_short;
  z_max_ = config.laser_z_max;
  z_rand_ = config.laser_z_rand;
  sigma_hit_ = config.laser_sigma_hit;
  lambda_short_ = config.laser_lambda_short;
  laser_likelihood_max_dist_ = config.laser_likelihood_max_dist;
 
  if(config.laser_model_type == "beam")
    laser_model_type_ = LASER_MODEL_BEAM;
  else if(config.laser_model_type == "likelihood_field")
    laser_model_type_ = LASER_MODEL_LIKELIHOOD_FIELD;
  else if(config.laser_model_type == "likelihood_field_prob")
    laser_model_type_ = LASER_MODEL_LIKELIHOOD_FIELD_PROB;
 
  if(config.odom_model_type == "diff")
    odom_model_type_ = ODOM_MODEL_DIFF;
  else if(config.odom_model_type == "omni")
    odom_model_type_ = ODOM_MODEL_OMNI;
  else if(config.odom_model_type == "diff-corrected")
    odom_model_type_ = ODOM_MODEL_DIFF_CORRECTED;
  else if(config.odom_model_type == "omni-corrected")
    odom_model_type_ = ODOM_MODEL_OMNI_CORRECTED;
 
  if(config.min_particles > config.max_particles)
  {
    ROS_WARN("You've set min_particles to be greater than max particles, this isn't allowed so they'll be set to be equal.");
    config.max_particles = config.min_particles;
  }
 
  pf_model_.use_optimal_filter = config.use_optimal_filter;
  pf_model_.use_crossover_mutation = config.use_intelligent_filter;
  pf_model_.use_self_adaptive = config.use_self_adaptive;
  pf_model_.use_adaptive_sampling = config.use_kld_sampling;
  
  min_particles_ = config.min_particles;
  max_particles_ = config.max_particles;
  N_aux_particles_ = config.N_aux_particles;
  alpha_slow_ = config.recovery_alpha_slow;
  alpha_fast_ = config.recovery_alpha_fast;
  crossover_alpha_ = config.crossover_alpha;
  mutation_prob_ =  config.mutation_probability;
  tf_broadcast_ = config.tf_broadcast;
 
  do_beamskip_= config.do_beamskip; 
  beam_skip_distance_ = config.beam_skip_distance; 
  beam_skip_threshold_ = config.beam_skip_threshold; 
  
  // Clear queued laser objects so that their parameters get updated
  lasers_.clear();
  ldata_.clear();
  GMCLLaserData::sldata.clear();
  lasers_update_.clear();
  frame_to_laser_.clear();
 
  if( pf_ != NULL )
  {
    pf_free( pf_ );
    pf_ = NULL;
  }     
  pf_ = pf_alloc(min_particles_, max_particles_,N_aux_particles_,
                 alpha_slow_, alpha_fast_, crossover_alpha_, mutation_prob_,
                 (pf_init_model_fn_t)GmclNode::uniformPoseGenerator,
                 (void *)map_, (void *)energy_map_);
  pf_set_model_type(pf_, &pf_model_);
  
  ROS_INFO("general particel filter initialized : ");    
  if (pf_->model.use_optimal_filter)
  std::cout<<("\t\twith optimal filter on \n");
  else
  std::cout<<("\t\twith optimal filter off \n");
  if (pf_->model.use_crossover_mutation)
  std::cout<<("\t\twith intelligent filter on \n");
  else
  std::cout<<("\t\twith intelligent filter off \n");
  if (pf_->model.use_self_adaptive)
  std::cout<<("\t\twith self adaptive on \n");
  else
  std::cout<<("\t\twith self adaptive off \n");
  if (pf_->model.use_adaptive_sampling)
  std::cout<<("\t\twith kld sampling  on \n");  
  else  
  std::cout<<("\t\twith kld sampling  off \n"); 
  pf_set_selective_resampling(pf_, selective_resampling_);
  pf_err_ = config.kld_err; 
  pf_z_ = config.kld_z; 
  pf_->pop_err = pf_err_;
  pf_->pop_z = pf_z_;
 
  // Initialize the filter
  pf_vector_t pf_init_pose_mean = pf_vector_zero();
  pf_init_pose_mean.v[0] = last_published_pose.pose.pose.position.x;
  pf_init_pose_mean.v[1] = last_published_pose.pose.pose.position.y;
  pf_init_pose_mean.v[2] = tf2::getYaw(last_published_pose.pose.pose.orientation);
  pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
  pf_init_pose_cov.m[0][0] = last_published_pose.pose.covariance[6*0+0];
  pf_init_pose_cov.m[1][1] = last_published_pose.pose.covariance[6*1+1];
  pf_init_pose_cov.m[2][2] = last_published_pose.pose.covariance[6*5+5];
  pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);
  pf_init_ = false;
 
  // Instantiate the sensor objects
  // Odometry
  delete odom_;
  odom_ = new GMCLOdom();
  ROS_ASSERT(odom_);
  odom_->SetModel( odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ );
  // Laser
  delete laser_;
  laser_ = new GMCLLaser(max_beams_, map_);
  ROS_ASSERT(laser_);
  if(laser_model_type_ == LASER_MODEL_BEAM)
    laser_->SetModelBeam(z_hit_, z_short_, z_max_, z_rand_,
                         sigma_hit_, lambda_short_, 0.0);
  else if(laser_model_type_ == LASER_MODEL_LIKELIHOOD_FIELD_PROB){
    ROS_INFO("Initializing likelihood field model; this can take some time on large maps...");
    laser_->SetModelLikelihoodFieldProb(z_hit_, z_rand_, sigma_hit_,
                                        laser_likelihood_max_dist_, 
                                        do_beamskip_, beam_skip_distance_, 
                                        beam_skip_threshold_, beam_skip_error_threshold_);
    ROS_INFO("Done initializing likelihood field model with probabilities.");
  }
  else if(laser_model_type_ == LASER_MODEL_LIKELIHOOD_FIELD){
    ROS_INFO("Initializing likelihood field model; this can take some time on large maps...");
    laser_->SetModelLikelihoodField(z_hit_, z_rand_, sigma_hit_,
                                    laser_likelihood_max_dist_);
    ROS_INFO("Done initializing likelihood field model.");
  }
 
  odom_frame_id_ = stripSlash(config.odom_frame_id);
  base_frame_id_ = stripSlash(config.base_frame_id);
  global_frame_id_ = stripSlash(config.global_frame_id);
 
  delete laser_scan_filter_;
  laser_scan_filter_ = 
          new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*laser_scan_sub_,
                                                             *tf_,
                                                             odom_frame_id_,
                                                             100,
                                                             nh_);
  laser_scan_filter_->registerCallback(boost::bind(&GmclNode::laserReceived,
                                                   this, _1));
 
  initial_pose_sub_ = nh_.subscribe("initialpose", 2, &GmclNode::initialPoseReceived, this);
}
 
 
void GmclNode::runFromBag(const std::string &in_bag_fn, bool trigger_global_localization)
{
  rosbag::Bag bag;
  bag.open(in_bag_fn, rosbag::bagmode::Read);
  std::vector<std::string> topics;
  topics.push_back(std::string("tf"));
  std::string scan_topic_name = "base_scan"; // TODO determine what topic this actually is from ROS
  topics.push_back(scan_topic_name);
  rosbag::View view(bag, rosbag::TopicQuery(topics));
 
  ros::Publisher laser_pub = nh_.advertise<sensor_msgs::LaserScan>(scan_topic_name, 100);
  ros::Publisher tf_pub = nh_.advertise<tf2_msgs::TFMessage>("/tf", 100);
 
  // Sleep for a second to let all subscribers connect
  ros::WallDuration(1.0).sleep();
 
  ros::WallTime start(ros::WallTime::now());
 
  // Wait for map
  while (ros::ok())
  {
    {
      boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);
      if (map_)
      {
        ROS_INFO("Map is ready");
        break;
      }
    }
    ROS_INFO("Waiting for map...");
    ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(1.0));
  }
 
  if (trigger_global_localization)
  {
    std_srvs::Empty empty_srv;
    globalLocalizationCallback(empty_srv.request, empty_srv.response);
  }
 
  BOOST_FOREACH(rosbag::MessageInstance const msg, view)
  {
    if (!ros::ok())
    {
      break;
    }
 
    // Process any ros messages or callbacks at this point
    ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration());
 
    tf2_msgs::TFMessage::ConstPtr tf_msg = msg.instantiate<tf2_msgs::TFMessage>();
    if (tf_msg != NULL)
    {
      tf_pub.publish(msg);
      for (size_t ii=0; ii<tf_msg->transforms.size(); ++ii)
      {
        tf_->setTransform(tf_msg->transforms[ii], "rosbag_authority");
      }
      continue;
    }
 
    sensor_msgs::LaserScan::ConstPtr base_scan = msg.instantiate<sensor_msgs::LaserScan>();
    if (base_scan != NULL)
    {
      laser_pub.publish(msg);
      laser_scan_filter_->add(base_scan);
      if (bag_scan_period_ > ros::WallDuration(0))
      {
        bag_scan_period_.sleep();
      }
      continue;
    }
 
    ROS_WARN_STREAM("Unsupported message type" << msg.getTopic());
  }
 
  bag.close();
 
  double runtime = (ros::WallTime::now() - start).toSec();
  ROS_INFO("Bag complete, took %.1f seconds to process, shutting down", runtime);
 
  const geometry_msgs::Quaternion & q(last_published_pose.pose.pose.orientation);
  double yaw, pitch, roll;
  tf2::Matrix3x3(tf2::Quaternion(q.x, q.y, q.z, q.w)).getEulerYPR(yaw,pitch,roll);
  ROS_INFO("Final location %.3f, %.3f, %.3f with stamp=%f",
            last_published_pose.pose.pose.position.x,
            last_published_pose.pose.pose.position.y,
            yaw, last_published_pose.header.stamp.toSec()
            );
 
  ros::shutdown();
}
 
 
void GmclNode::savePoseToServer()
{
  // We need to apply the last transform to the latest odom pose to get
  // the latest map pose to store.  We'll take the covariance from
  // last_published_pose.
  tf2::Transform odom_pose_tf2;
  tf2::convert(latest_odom_pose_.pose, odom_pose_tf2);
  tf2::Transform map_pose = latest_tf_.inverse() * odom_pose_tf2;
 
  double yaw = tf2::getYaw(map_pose.getRotation());
 
  ROS_DEBUG("Saving pose to server. x: %.3f, y: %.3f", map_pose.getOrigin().x(), map_pose.getOrigin().y() );
 
  private_nh_.setParam("initial_pose_x", map_pose.getOrigin().x());
  private_nh_.setParam("initial_pose_y", map_pose.getOrigin().y());
  private_nh_.setParam("initial_pose_a", yaw);
  private_nh_.setParam("initial_cov_xx", 
                                  last_published_pose.pose.covariance[6*0+0]);
  private_nh_.setParam("initial_cov_yy", 
                                  last_published_pose.pose.covariance[6*1+1]);
  private_nh_.setParam("initial_cov_aa", 
                                  last_published_pose.pose.covariance[6*5+5]);
}
 
void GmclNode::updatePoseFromServer()
{
  init_pose_[0] = 0.0;
  init_pose_[1] = 0.0;
  init_pose_[2] = 0.0;
  init_cov_[0] = 0.5 * 0.5;
  init_cov_[1] = 0.5 * 0.5;
  init_cov_[2] = (M_PI/12.0) * (M_PI/12.0);
  // Check for NAN on input from param server, #5239
  double tmp_pos;
  private_nh_.param("initial_pose_x", tmp_pos, init_pose_[0]);
  if(!std::isnan(tmp_pos))
    init_pose_[0] = tmp_pos;
  else 
    ROS_WARN("ignoring NAN in initial pose X position");
  private_nh_.param("initial_pose_y", tmp_pos, init_pose_[1]);
  if(!std::isnan(tmp_pos))
    init_pose_[1] = tmp_pos;
  else
    ROS_WARN("ignoring NAN in initial pose Y position");
  private_nh_.param("initial_pose_a", tmp_pos, init_pose_[2]);
  if(!std::isnan(tmp_pos))
    init_pose_[2] = tmp_pos;
  else
    ROS_WARN("ignoring NAN in initial pose Yaw");
  private_nh_.param("initial_cov_xx", tmp_pos, init_cov_[0]);
  if(!std::isnan(tmp_pos))
    init_cov_[0] =tmp_pos;
  else
    ROS_WARN("ignoring NAN in initial covariance XX");
  private_nh_.param("initial_cov_yy", tmp_pos, init_cov_[1]);
  if(!std::isnan(tmp_pos))
    init_cov_[1] = tmp_pos;
  else
    ROS_WARN("ignoring NAN in initial covariance YY");
  private_nh_.param("initial_cov_aa", tmp_pos, init_cov_[2]);
  if(!std::isnan(tmp_pos))
    init_cov_[2] = tmp_pos;
  else
    ROS_WARN("ignoring NAN in initial covariance AA");  
}
 
void 
GmclNode::checkLaserReceived(const ros::TimerEvent& event)
{
  ros::Duration d = ros::Time::now() - last_laser_received_ts_;
  if(d > laser_check_interval_)
  {
    ROS_WARN("No laser scan received (and thus no pose updates have been published) for %f seconds.  Verify that data is being published on the %s topic.",
             d.toSec(),
             ros::names::resolve(scan_topic_).c_str());
  }
}
 
void
GmclNode::requestMap()
{
  boost::recursive_mutex::scoped_lock ml(configuration_mutex_);
 
  // get map via RPC
  nav_msgs::GetMap::Request  req;
  nav_msgs::GetMap::Response resp;
  ROS_INFO("Requesting the map...");
  while(!ros::service::call("static_map", req, resp))
  {
    ROS_WARN("Request for map failed; trying again...");
    ros::Duration d(0.5);
    d.sleep();
  }
  handleMapMessage( resp.map );
}
 
void
GmclNode::mapReceived(const nav_msgs::OccupancyGridConstPtr& msg)
{
  if( first_map_only_ && first_map_received_ ) {
    return;
  }
 
  handleMapMessage( *msg );
 
  first_map_received_ = true;
}
 
void
GmclNode::handleMapMessage(const nav_msgs::OccupancyGrid& msg)
{
  boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);
 
  ROS_INFO("Received a %d X %d map @ %.3f m/pix\n",
           msg.info.width,
           msg.info.height,
           msg.info.resolution);
  
  if(msg.header.frame_id != global_frame_id_)
    ROS_WARN("Frame_id of map received:'%s' doesn't match global_frame_id:'%s'. This could cause issues with reading published topics",
             msg.header.frame_id.c_str(),
             global_frame_id_.c_str());
 
  freeMapDependentMemory();
  // Clear queued laser objects because they hold pointers to the existing
  // map, #5202.
  lasers_.clear();
  ldata_.clear();
  GMCLLaserData::sldata.clear();
  lasers_update_.clear();
  frame_to_laser_.clear();
 
  map_ = convertMap(msg);
  if(pf_model_.use_self_adaptive)
   {
  energy_map_ = energy_map_alloc(map_ ,resolution_x_ ,resolution_y_ , 
                                               max_beams_,laser_min_range_,laser_max_range_); 
 
  ROS_INFO("energy map has been created with a %d X %d \n energy cell @ resoultion_x %.2f m and resoultion_y %.2f m \n",
                           energy_map_->size_x, energy_map_->size_y, energy_map_->resoultion_x , energy_map_->resoultion_y);
  }        
          
#if NEW_UNIFORM_SAMPLING
  // Index of free space
  free_space_indices.resize(0);
  for(int i = 0; i < map_->size_x; i++)
    for(int j = 0; j < map_->size_y; j++)
      if(map_->cells[MAP_INDEX(map_,i,j)].occ_state == -1)
        free_space_indices.push_back(std::make_pair(i,j));
#endif
  // Create the particle filter
  pf_ = pf_alloc(min_particles_, max_particles_,N_aux_particles_,
                 alpha_slow_, alpha_fast_, crossover_alpha_ , mutation_prob_ ,
                 (pf_init_model_fn_t)GmclNode::uniformPoseGenerator,
                 (void *)map_,  (void *)energy_map_);
  pf_set_model_type(pf_, &pf_model_);  
  ROS_INFO("general particle filter initialized : ");    
  if (pf_->model.use_optimal_filter)
  std::cout<<("\t\twith optimal filter on \n");
  else
  std::cout<<("\t\twith optimal filter off \n");
  if (pf_->model.use_crossover_mutation)
  std::cout<<("\t\twith intelligent filter on \n");
  else
  std::cout<<("\t\twith intelligent filter off \n");
  if (pf_->model.use_self_adaptive)
  std::cout<<("\t\twith self adaptive on \n");
  else
  std::cout<<("\t\twith self adaptive off \n");
  if (pf_->model.use_adaptive_sampling)
  std::cout<<("\t\twith kld sampling  on \n");  
  else  
  std::cout<<("\t\twith kld sampling  off \n"); 
                       
  pf_set_selective_resampling(pf_, selective_resampling_);
  pf_->pop_err = pf_err_;
  pf_->pop_z = pf_z_;
 
  // Initialize the filter
  updatePoseFromServer();
  pf_vector_t pf_init_pose_mean = pf_vector_zero();
  pf_init_pose_mean.v[0] = init_pose_[0];
  pf_init_pose_mean.v[1] = init_pose_[1];
  pf_init_pose_mean.v[2] = init_pose_[2];
  pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
  pf_init_pose_cov.m[0][0] = init_cov_[0];
  pf_init_pose_cov.m[1][1] = init_cov_[1];
  pf_init_pose_cov.m[2][2] = init_cov_[2];
  pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);
  pf_init_ = false;
 
  // Instantiate the sensor objects
  // Odometry
  delete odom_;
  odom_ = new GMCLOdom();
  ROS_ASSERT(odom_);
  odom_->SetModel( odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ );
  // Laser
  delete laser_;
  laser_ = new GMCLLaser(max_beams_, map_);
  ROS_ASSERT(laser_);
  if(laser_model_type_ == LASER_MODEL_BEAM)
    laser_->SetModelBeam(z_hit_, z_short_, z_max_, z_rand_,
                         sigma_hit_, lambda_short_, 0.0);
  else if(laser_model_type_ == LASER_MODEL_LIKELIHOOD_FIELD_PROB){
    ROS_INFO("Initializing likelihood field model; this can take some time on large maps...");
    laser_->SetModelLikelihoodFieldProb(z_hit_, z_rand_, sigma_hit_,
                                        laser_likelihood_max_dist_, 
                                        do_beamskip_, beam_skip_distance_, 
                                        beam_skip_threshold_, beam_skip_error_threshold_);
    ROS_INFO("Done initializing likelihood field model.");
  }
  else
  {
    ROS_INFO("Initializing likelihood field model; this can take some time on large maps...");
    laser_->SetModelLikelihoodField(z_hit_, z_rand_, sigma_hit_,
                                    laser_likelihood_max_dist_);
    ROS_INFO("Done initializing likelihood field model.");
  }
 
  // In case the initial pose message arrived before the first map,
  // try to apply the initial pose now that the map has arrived.
  applyInitialPose();
 
}
 
void
GmclNode::freeMapDependentMemory()
{
  if( map_ != NULL ) {
    map_free( map_ );
    map_ = NULL;
  }
  if( energy_map_ != NULL ) {
    energy_map_free( energy_map_ );
    energy_map_ = NULL;
  }
  if( pf_ != NULL ) {
    pf_free( pf_ );
    pf_ = NULL;
  }
  delete odom_;
  odom_ = NULL;
  delete laser_;
  laser_ = NULL;
}
 
map_t*
GmclNode::convertMap( const nav_msgs::OccupancyGrid& map_msg )
{
  map_t* map = map_alloc();
  ROS_ASSERT(map);
 
  map->size_x = map_msg.info.width;
  map->size_y = map_msg.info.height;
  map->scale = map_msg.info.resolution;
  map->origin_x = map_msg.info.origin.position.x + (map->size_x / 2) * map->scale;
  map->origin_y = map_msg.info.origin.position.y + (map->size_y / 2) * map->scale;
  // Convert to player format
  map->cells = (map_cell_t*)malloc(sizeof(map_cell_t)*map->size_x*map->size_y);
  ROS_ASSERT(map->cells);
  for(int i=0;i<map->size_x * map->size_y;i++)
  {
    if(map_msg.data[i] == 0)
      map->cells[i].occ_state = -1;
    else if(map_msg.data[i] == 100)
      map->cells[i].occ_state = +1;
    else
      map->cells[i].occ_state = 0;
  }
 
  return map;
}
 
GmclNode::~GmclNode()
{
  delete dsrv_;
  freeMapDependentMemory();
  delete laser_scan_filter_;
  delete laser_scan_sub_;
  // TODO: delete everything allocated in constructor
}
 
bool
GmclNode::getOdomPose(geometry_msgs::PoseStamped& odom_pose,
                      double& x, double& y, double& yaw,
                      const ros::Time& t, const std::string& f)
{
  // Get the robot's pose
  geometry_msgs::PoseStamped ident;
  ident.header.frame_id = stripSlash(f);
  ident.header.stamp = t;
  tf2::toMsg(tf2::Transform::getIdentity(), ident.pose);
  try
  {
    this->tf_->transform(ident, odom_pose, odom_frame_id_);
  }
  catch(tf2::TransformException e)
  {
    ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
    return false;
  }
  x = odom_pose.pose.position.x;
  y = odom_pose.pose.position.y;
  yaw = tf2::getYaw(odom_pose.pose.orientation);
 
  return true;
}
 
 
pf_vector_t
GmclNode::uniformPoseGenerator(pf_t *pf, void* arg , void* e_arg )
{
if(pf->model.use_optimal_filter && GMCLLaserData::sldata.size()>0)   
{  
 double max_weight = 0;
 pf_vector_t f_pose;
 pf_vector_t t_p[pf->N_aux_particles];
 GMCLLaser *self; 
 if(pf->model.use_self_adaptive)
  {  energy_map_t* energy_map = (energy_map_t*)e_arg;
   if(energy_map->accepted_count>0)
   {      
   //         ROS_INFO("size %d", AMCLLaserData::sldata.size());  
      for ( int loop=0 ; loop < pf->N_aux_particles ; loop++)
     {    int index = drand48()*energy_map->accepted_count;
          t_p[loop].v[0] = energy_map->poses[energy_map->accepted_index[index]].pose_x ;
          t_p[loop].v[1] = energy_map->poses[energy_map->accepted_index[index]].pose_y ;
          t_p[loop].v[2] = energy_map->poses[energy_map->accepted_index[index]].orientation_yaw;
          double pose_weight = 1.0 ;
          
          for ( int  laser_index = 0 ; laser_index < GMCLLaserData::sldata.size() ; laser_index++ )
          {    
               self = (GMCLLaser*) GMCLLaserData::sldata[laser_index]->sensor;
               pose_weight *= self->PoseWeight(t_p[loop] , GMCLLaserData::sldata[laser_index]);
            //   ROS_INFO(" self adaptive aux weight%f", pose_weight);  
 
          }
          if ( pose_weight > max_weight)
          { 
              f_pose = t_p[loop];
              max_weight = pose_weight;          
          }
 
    }
  //    ROS_INFO(" self adaptive max weight %f", max_weight);     
      return f_pose;   
   }
  }
  map_t* map = (map_t*)arg; 
  
int loop ;
 
#if NEW_UNIFORM_SAMPLING
 
 for (  loop=0 ; loop < pf->N_aux_particles ; loop++)
 {
  unsigned int rand_index = drand48() * free_space_indices.size();
  std::pair<int,int> free_point = free_space_indices[rand_index];
 
  t_p[loop].v[0] = MAP_WXGX(map, free_point.first);
  t_p[loop].v[1] = MAP_WYGY(map, free_point.second);
  t_p[loop].v[2] = drand48() * 2 * M_PI - M_PI;
#else
  double min_x, max_x, min_y, max_y;
 
  min_x = (map->size_x * map->scale)/2.0 - map->origin_x;
  max_x = (map->size_x * map->scale)/2.0 + map->origin_x;
  min_y = (map->size_y * map->scale)/2.0 - map->origin_y;
  max_y = (map->size_y * map->scale)/2.0 + map->origin_y;
 
 for (  loop=0 ; loop < pf->N_aux_particles ; loop++)
  {
  ROS_DEBUG("Generating new uniform sample");
  for(;;)
  {
    t_p[loop].v[0] = min_x + drand48() * (max_x - min_x);
    t_p[loop].v[1] = min_y + drand48() * (max_y - min_y);
    t_p[loop].v[2] = drand48() * 2 * M_PI - M_PI;
    // Check that it's a free cell
    int i,j;
    i = MAP_GXWX(map, p.v[0]);
    j = MAP_GYWY(map, p.v[1]);
    if(MAP_VALID(map,i,j) && (map->cells[MAP_INDEX(map,i,j)].occ_state == -1))
      break;
  }
 #endif
    
       double pose_weight = 1.0 ;
         for ( int  laser_index = 0 ; laser_index < GMCLLaserData::sldata.size() ; laser_index++ )
          {    
               self = (GMCLLaser*) GMCLLaserData::sldata[laser_index]->sensor;
               pose_weight *= self->PoseWeight(t_p[loop] , GMCLLaserData::sldata[laser_index]);
 
          }
          if ( pose_weight > max_weight)
          { 
              f_pose = t_p[loop];
             max_weight = pose_weight;      
          }  
  // ROS_INFO("Generating new uniform sample %d", loop);     
  }
 
 return f_pose;
 
}
else
 { if(pf->model.use_self_adaptive)
 {  energy_map_t* energy_map = (energy_map_t*)e_arg;
    if(energy_map->accepted_count>0)
     { 
      pf_vector_t p;
      int index = drand48()*energy_map->accepted_count;
       p.v[0] = energy_map->poses[energy_map->accepted_index[index]].pose_x ;
       p.v[1] = energy_map->poses[energy_map->accepted_index[index]].pose_y ;
       p.v[2] = energy_map->poses[energy_map->accepted_index[index]].orientation_yaw;
     
      return p;   
     }
 } 
  map_t* map = (map_t*)arg; 
#if NEW_UNIFORM_SAMPLING
  unsigned int rand_index = drand48() * free_space_indices.size();
  std::pair<int,int> free_point = free_space_indices[rand_index];
  pf_vector_t p;
  p.v[0] = MAP_WXGX(map, free_point.first);
  p.v[1] = MAP_WYGY(map, free_point.second);
  p.v[2] = drand48() * 2 * M_PI - M_PI;
#else
  double min_x, max_x, min_y, max_y;
 
  min_x = (map->size_x * map->scale)/2.0 - map->origin_x;
  max_x = (map->size_x * map->scale)/2.0 + map->origin_x;
  min_y = (map->size_y * map->scale)/2.0 - map->origin_y;
  max_y = (map->size_y * map->scale)/2.0 + map->origin_y;
 
  pf_vector_t p;
 
  ROS_DEBUG("Generating new uniform sample");
  for(;;)
  {
    p.v[0] = min_x + drand48() * (max_x - min_x);
    p.v[1] = min_y + drand48() * (max_y - min_y);
    p.v[2] = drand48() * 2 * M_PI - M_PI;
    // Check that it's a free cell
    int i,j;
    i = MAP_GXWX(map, p.v[0]);
    j = MAP_GYWY(map, p.v[1]);
    if(MAP_VALID(map,i,j) && (map->cells[MAP_INDEX(map,i,j)].occ_state == -1))
      break;
  }
#endif
 return p;
 
 }
}
bool
GmclNode::globalLocalizationCallback(std_srvs::Empty::Request& req,
                                     std_srvs::Empty::Response& res)
{
  if( map_ == NULL ) {
    return true;
  }
  
  boost::recursive_mutex::scoped_lock gl(configuration_mutex_);
  
  if(!pf_->model.use_self_adaptive)
  { 
     ROS_INFO("Initializing with uniform distribution");
  }
  else
   ROS_INFO("Initializing with uniform distribution in energy map");
  
  pf_init_model(pf_, (pf_init_model_fn_t)GmclNode::uniformPoseGenerator,
                (void *)map_ , (void *)energy_map_ );
  ROS_INFO("Global initialisation done!");
  pf_init_ = false;
  return true;
}
 
// force nomotion updates (amcl updating without requiring motion)
bool 
GmclNode::nomotionUpdateCallback(std_srvs::Empty::Request& req,
                                     std_srvs::Empty::Response& res)
{
        m_force_update = true;
        //ROS_INFO("Requesting no-motion update");
        return true;
}
 
bool
GmclNode::setMapCallback(nav_msgs::SetMap::Request& req,
                         nav_msgs::SetMap::Response& res)
{
  handleMapMessage(req.map);
  handleInitialPoseMessage(req.initial_pose);
  res.success = true;
  return true;
}
 
 
 
void
GmclNode::handelNewLaser(const sensor_msgs::LaserScanConstPtr& laser_scan)
{   
  lasers_.push_back(new GMCLLaser(*laser_));
  ldata_.push_back(new GMCLLaserData); 
  GMCLLaserData::sldata.push_back(new GMCLLaserData);        
  lasers_update_.push_back(true);
  laser_index = frame_to_laser_.size();
  ldata_[laser_index]->sensor = lasers_[laser_index]; 
  lsensor_scan_t t_laser_scan;
  
  std::string laser_scan_frame_id = stripSlash(laser_scan->header.frame_id);
  geometry_msgs::PoseStamped ident;
  ident.header.frame_id = laser_scan_frame_id;
  ident.header.stamp = ros::Time();
  tf2::toMsg(tf2::Transform::getIdentity(), ident.pose);
  geometry_msgs::PoseStamped laser_pose;
  try
  {
    this->tf_->transform(ident, laser_pose, base_frame_id_);
  }
  catch(tf2::TransformException& e)
  {
    ROS_ERROR("Couldn't transform from %s to %s, "
              "even though the message notifier is in use",
              laser_scan_frame_id.c_str(),
              base_frame_id_.c_str());
    return;
  }
 
  pf_vector_t laser_pose_v;
  laser_pose_v.v[0] = laser_pose.pose.position.x;
  laser_pose_v.v[1] = laser_pose.pose.position.y;
  // laser mounting angle gets computed later -> set to 0 here!
  laser_pose_v.v[2] = 0;
      
  ROS_DEBUG("Received laser's pose wrt robot: %.3f %.3f %.3f",
            laser_pose_v.v[0],
            laser_pose_v.v[1],
            laser_pose_v.v[2]);
                        
  t_laser_scan.laser_pose = laser_pose_v; 
    
  frame_to_laser_[laser_scan_frame_id] = laser_index;
 
  tf2::Quaternion q;
  q.setRPY(0.0, 0.0, laser_scan->angle_min);
  geometry_msgs::QuaternionStamped min_q, inc_q;
  min_q.header.stamp = laser_scan->header.stamp;
  min_q.header.frame_id = stripSlash(laser_scan->header.frame_id);
  tf2::convert(q, min_q.quaternion);
 
  q.setRPY(0.0, 0.0, laser_scan->angle_min + laser_scan->angle_increment);
  inc_q.header = min_q.header;
  tf2::convert(q, inc_q.quaternion);
  try
  {
    tf_->transform(min_q, min_q, base_frame_id_);
    tf_->transform(inc_q, inc_q, base_frame_id_);
  }
  catch(tf2::TransformException& e)
  {
    ROS_WARN("Unable to transform min/max laser angles into base frame: %s",
             e.what());
    return;
  } 
  double angle_min = tf2::getYaw(min_q.quaternion);
  double angle_increment = tf2::getYaw(inc_q.quaternion) - angle_min;
 
  // wrapping angle to [-pi .. pi]
  angle_increment = fmod(angle_increment + 5*M_PI, 2*M_PI) - M_PI; //angle wrt base
       
   // Apply range min/max thresholds, if the user supplied them
  double range_max , range_min ;
  
  if(laser_max_range_ > 0.0)
      range_max = std::min(laser_scan->range_max, (float)laser_max_range_);
  else
      range_max = laser_scan->range_max;
       
  if(laser_min_range_ > 0.0)
      range_min = std::max(laser_scan->range_min, (float)laser_min_range_);
  else
      range_min = laser_scan->range_min;
  
  t_laser_scan.range_max = range_max;
  t_laser_scan.range_min = range_min;
  t_laser_scan.range_count = laser_scan->ranges.size();
  
  // The AMCLLaserData destructor will free this memory
  ldata_[laser_index]->ranges = new double[laser_scan->ranges.size()][2];    
  ROS_ASSERT(ldata_[laser_index]->ranges);
  
  for(int i=0; i<laser_scan->ranges.size(); i++)
  
  ldata_[laser_index]->ranges[i][1] = angle_min + (i * angle_increment);
  
  t_laser_scan.ranges = ldata_[laser_index]->ranges ;
  
  lasers_[laser_index]->SetLaserScan(t_laser_scan) ;
   
  ldata_[laser_index]->range_count = t_laser_scan.range_count;   
 
  if(pf_->model.use_self_adaptive)    
  { 
    energy_map_->threshold_val = threshold_val_;
    
    ROS_INFO("adding Laser %d to energy map; this can take some time on large maps with high resolution...", laser_index); 
    
    map_update_espace(energy_map_,map_, &t_laser_scan) ;   // compute the espace for new laser
    
 
                                               
    ROS_INFO("Laser %d has been added to espace", laser_index);            
  }
  
}
 
void
GmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan)
{
  std::string laser_scan_frame_id = stripSlash(laser_scan->header.frame_id);
  last_laser_received_ts_ = ros::Time::now();
  if( map_ == NULL ) {
    return;
  }
  boost::recursive_mutex::scoped_lock lr(configuration_mutex_);
  
  // Do we have the base->base_laser Tx yet?
  if(frame_to_laser_.find(laser_scan_frame_id) == frame_to_laser_.end())
  {
    ROS_INFO("Setting up laser %d (frame_id=%s)\n", (int)frame_to_laser_.size(), laser_scan_frame_id.c_str());
    
    GmclNode::handelNewLaser(laser_scan) ;
   
  } else {
    // we have the laser pose, retrieve laser index
    laser_index = frame_to_laser_[laser_scan_frame_id];
  }
 
  // Where was the robot when this scan was taken?
  pf_vector_t pose;
  if(!getOdomPose(latest_odom_pose_, pose.v[0], pose.v[1], pose.v[2],
                  laser_scan->header.stamp, base_frame_id_))
  {
    ROS_ERROR("Couldn't determine robot's pose associated with laser scan");
    return;
  }
 
 
  pf_vector_t delta = pf_vector_zero();
 
  if(pf_init_)
  {
    // Compute change in pose
    //delta = pf_vector_coord_sub(pose, pf_odom_pose_);
    delta.v[0] = pose.v[0] - pf_odom_pose_.v[0];
    delta.v[1] = pose.v[1] - pf_odom_pose_.v[1];
    delta.v[2] = angle_diff(pose.v[2], pf_odom_pose_.v[2]);
 
if (  fabs (delta.v[0] )> 2.0 || fabs (delta.v[1] )> 2.0)
   {  pf_odom_pose_.v[0] =  pose.v[0] ;
      pf_odom_pose_.v[1] =  pose.v[1] ;
      pf_odom_pose_.v[2] = pose.v[2] ;
   return;
   }
    // See if we should update the filter
    bool update = fabs(delta.v[0]) > d_thresh_ ||
                  fabs(delta.v[1]) > d_thresh_ ||
                  fabs(delta.v[2]) > a_thresh_;
    update = update || m_force_update;
    m_force_update=false;
 
    // Set the laser update flags
    if(update)
      for(unsigned int i=0; i < lasers_update_.size(); i++)
        lasers_update_[i] = true;
  }
 
  bool force_publication = false;
  if(!pf_init_)
  {
    // Pose at last filter update
    pf_odom_pose_ = pose;
 
    // Filter is now initialized
    pf_init_ = true;
 
    // Should update sensor data
    for(unsigned int i=0; i < lasers_update_.size(); i++)
      lasers_update_[i] = true;
 
    force_publication = true;
 
    resample_count_ = 0;
  }
  // If the robot has moved, update the filter
  else if(pf_init_ && lasers_update_[laser_index])
  {
    //printf("pose\n");
    //pf_vector_fprintf(pose, stdout, "%.3f");
 
    GMCLOdomData odata;
    odata.sensor = odom_;
    odata.pose = pose;
    // HACK
    // Modify the delta in the action data so the filter gets
    // updated correctly
    odata.delta = delta;
 
    // Use the action data to update the filter
    odom_->UpdateAction(pf_, (GMCLSensorData*)&odata);
 
    // Pose at last filter update
    //this->pf_odom_pose = pose;
  }
 
  bool resampled = false;
  // If the robot has moved, update the filter
  if(lasers_update_[laser_index])
  {
    
    // To account for lasers that are mounted upside-down, we determine the
    // min, max, and increment angles of the laser in the base frame.
    //
    // Construct min and max angles of laser, in the base_link frame.
    
    for(int i=0;i<laser_scan->ranges.size();i++)
    {
        
      // amcl doesn't (yet) have a concept of min range.  So we'll map short
      // readings to max range.
      if(laser_scan->ranges[i] <= lasers_[laser_index]->range_min)        
        ldata_[laser_index]->ranges[i][0] = lasers_[laser_index]->range_max;
        
     // else if (laser_scan->ranges[i] >= lasers_[laser_index]->range_max)            
     //   ldata_[laser_index]->ranges[i][0] = lasers_[laser_index]->range_max;                
      else      
        ldata_[laser_index]->ranges[i][0] = laser_scan->ranges[i];
     
    }
    
    GMCLLaserData::sldata[laser_index]  = ldata_[laser_index] ;
    lasers_[laser_index]->UpdateSensor(pf_,  (GMCLSensorData*) GMCLLaserData::sldata[laser_index]);
 
    lasers_update_[laser_index] = false;
 
    pf_odom_pose_ = pose; 
                                     
   if(pf_->model.use_crossover_mutation)   
       pf_update_crossover_mutation(pf_, (pf_reupdate_sensor_fn_t) GMCLLaser::ReUpdateSensor,
                                                            (GMCLSensorData*)ldata_[laser_index] );   
   if(pf_->model.use_self_adaptive)
    { 
     lsensor_scan_t* t_laser_scan = new lsensor_scan_t;
    
     t_laser_scan->ranges = ldata_[laser_index]->ranges;
     t_laser_scan->range_count = ldata_[laser_index]->range_count;
     t_laser_scan->range_max = lasers_[laser_index]->range_max;
     
     map_calc_SER(energy_map_ , t_laser_scan);
     
     delete t_laser_scan;    
     if(!((resample_count_+1) % lasers_update_.size()))                                                     
         map_diff_SER(energy_map_);        
    }                                                             
                                                                    
    // Resample the particles
    if(!(++resample_count_ % resample_interval_)) // must collect data from all laser data sensors
    {                                                                                    // before resampling        
       pf_update_resample(pf_);
       resampled = true;
    }
  
          
    pf_sample_set_t* set = pf_->sets + pf_->current_set;                  
    ROS_INFO("Num samples: %d\n", set->sample_count);
    // Publish the resulting cloud
    // TODO: set maximum rate for publishing
    if (!m_force_update)
    {       
      if(pub_energy_map_ && pf_->model.use_self_adaptive)     
      {  energy_pose_t* poses = energy_map_->poses;
         int* accepted_index =  energy_map_->accepted_index;
         int accepted_count = energy_map_->accepted_count ;
         geometry_msgs::PoseArray energy_cloud_msg;
         energy_cloud_msg.header.stamp = ros::Time::now();
         energy_cloud_msg.header.frame_id = global_frame_id_;
         energy_cloud_msg.poses.resize(energy_map_->accepted_count);
        
        for (int i=0 ; i<accepted_count ; i++ )
        {          
          energy_cloud_msg.poses[i].position.x = poses[accepted_index[i]].pose_x;
          energy_cloud_msg.poses[i].position.y = poses[accepted_index[i]].pose_y;
          energy_cloud_msg.poses[i].position.z = 0;
          tf2::Quaternion q;
          q.setRPY(0, 0,poses[accepted_index[i]].orientation_yaw);
          tf2::convert(q,energy_cloud_msg.poses[i].orientation);           
        }
       energy_particlecloud_pub_.publish(energy_cloud_msg);             
      }  
      
      geometry_msgs::PoseArray cloud_msg;      
      cloud_msg.header.stamp = ros::Time::now();
      cloud_msg.header.frame_id = global_frame_id_;
      cloud_msg.poses.resize(set->sample_count);
    
      for(int i=0;i<set->sample_count;i++)
      {
        cloud_msg.poses[i].position.x = set->samples[i].pose.v[0];
        cloud_msg.poses[i].position.y = set->samples[i].pose.v[1];
        cloud_msg.poses[i].position.z = 0;
        tf2::Quaternion q;
        q.setRPY(0, 0, set->samples[i].pose.v[2]);
        tf2::convert(q, cloud_msg.poses[i].orientation);
      }
      particlecloud_pub_.publish(cloud_msg);
     
   
    }
  }
  if(resampled || force_publication)
  {
    // Read out the current hypotheses
    double max_weight = 0.0;
    int max_weight_hyp = -1;
    std::vector<gmcl_hyp_t> hyps;
    hyps.resize(pf_->sets[pf_->current_set].cluster_count);
    for(int hyp_count = 0;
        hyp_count < pf_->sets[pf_->current_set].cluster_count; hyp_count++)
    {
      double weight;
      pf_vector_t pose_mean;
      pf_matrix_t pose_cov;
      if (!pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean, &pose_cov))
      {
        ROS_ERROR("Couldn't get stats on cluster %d", hyp_count);
        break;
      }
      hyps[hyp_count].weight = weight;
      hyps[hyp_count].pf_pose_mean = pose_mean;
      hyps[hyp_count].pf_pose_cov = pose_cov;
 
      if(hyps[hyp_count].weight > max_weight)
      {
        max_weight = hyps[hyp_count].weight;
        max_weight_hyp = hyp_count;
      }
    }
 
    if(max_weight > 0.0)
    {
      ROS_DEBUG("Max weight pose: %.3f %.3f %.3f",
                hyps[max_weight_hyp].pf_pose_mean.v[0],
                hyps[max_weight_hyp].pf_pose_mean.v[1],
                hyps[max_weight_hyp].pf_pose_mean.v[2]);
 
      /*
         puts("");
         pf_matrix_fprintf(hyps[max_weight_hyp].pf_pose_cov, stdout, "%6.3f");
         puts("");
       */
 
      geometry_msgs::PoseWithCovarianceStamped p;
      // Fill in the header
      p.header.frame_id = global_frame_id_;
      p.header.stamp = laser_scan->header.stamp;
      // Copy in the pose
      p.pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0];
      p.pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1];
 
      tf2::Quaternion q;
      q.setRPY(0, 0, hyps[max_weight_hyp].pf_pose_mean.v[2]);
      tf2::convert(q, p.pose.pose.orientation);
      // Copy in the covariance, converting from 3-D to 6-D
      pf_sample_set_t* set = pf_->sets + pf_->current_set;
      for(int i=0; i<2; i++)
      {
        for(int j=0; j<2; j++)
        {
          // Report the overall filter covariance, rather than the
          // covariance for the highest-weight cluster
          //p.covariance[6*i+j] = hyps[max_weight_hyp].pf_pose_cov.m[i][j];
          p.pose.covariance[6*i+j] = set->cov.m[i][j];
        }
      }
      p.pose.covariance[6*3] = set->cov.m[2][0];
      // Report the overall filter covariance, rather than the
      // covariance for the highest-weight cluster
      //p.covariance[6*5+5] = hyps[max_weight_hyp].pf_pose_cov.m[2][2];
      p.pose.covariance[6*5+5] = set->cov.m[2][2];
 
      /*
         printf("cov:\n");
         for(int i=0; i<6; i++)
         {
         for(int j=0; j<6; j++)
         printf("%6.3f ", p.covariance[6*i+j]);
         puts("");
         }
       */
 
      pose_pub_.publish(p);
      last_published_pose = p;
 
      ROS_DEBUG("New pose: %6.3f %6.3f %6.3f",
               hyps[max_weight_hyp].pf_pose_mean.v[0],
               hyps[max_weight_hyp].pf_pose_mean.v[1],
               hyps[max_weight_hyp].pf_pose_mean.v[2]);
 
      // subtracting base to odom from map to base and send map to odom instead
      geometry_msgs::PoseStamped odom_to_map;
      try
      {
        tf2::Quaternion q;
        q.setRPY(0, 0, hyps[max_weight_hyp].pf_pose_mean.v[2]);
        tf2::Transform tmp_tf(q, tf2::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
                                              hyps[max_weight_hyp].pf_pose_mean.v[1],
                                              0.0));
 
        geometry_msgs::PoseStamped tmp_tf_stamped;
        tmp_tf_stamped.header.frame_id = base_frame_id_;
        tmp_tf_stamped.header.stamp = laser_scan->header.stamp;
        tf2::toMsg(tmp_tf.inverse(), tmp_tf_stamped.pose);
 
        this->tf_->transform(tmp_tf_stamped, odom_to_map, odom_frame_id_);
      }
      catch(tf2::TransformException)
      {
        ROS_DEBUG("Failed to subtract base to odom transform");
        return;
      }
 
      tf2::convert(odom_to_map.pose, latest_tf_);
      latest_tf_valid_ = true;
 
      if (tf_broadcast_ == true)
      {
        // We want to send a transform that is good up until a
        // tolerance time so that odom can be used
        ros::Time transform_expiration = (laser_scan->header.stamp +
                                          transform_tolerance_);
        geometry_msgs::TransformStamped tmp_tf_stamped;
        tmp_tf_stamped.header.frame_id = global_frame_id_;
        tmp_tf_stamped.header.stamp = transform_expiration;
        tmp_tf_stamped.child_frame_id = odom_frame_id_;
        tf2::convert(latest_tf_.inverse(), tmp_tf_stamped.transform);
 
        this->tfb_->sendTransform(tmp_tf_stamped);
        sent_first_transform_ = true;
      }
    }
    else
    {
      ROS_ERROR("No pose!");
    }
  }
  else if(latest_tf_valid_)
  {
    if (tf_broadcast_ == true)
    {
      // Nothing changed, so we'll just republish the last transform, to keep
      // everybody happy.
      ros::Time transform_expiration = (laser_scan->header.stamp +
                                        transform_tolerance_);
      geometry_msgs::TransformStamped tmp_tf_stamped;
      tmp_tf_stamped.header.frame_id = global_frame_id_;
      tmp_tf_stamped.header.stamp = transform_expiration;
      tmp_tf_stamped.child_frame_id = odom_frame_id_;
      tf2::convert(latest_tf_.inverse(), tmp_tf_stamped.transform);
      this->tfb_->sendTransform(tmp_tf_stamped);
    }
 
    // Is it time to save our last pose to the param server
    ros::Time now = ros::Time::now();
    if((save_pose_period.toSec() > 0.0) &&
       (now - save_pose_last_time) >= save_pose_period)
    {
      this->savePoseToServer();
      save_pose_last_time = now;
    }
  }
 
  diagnosic_updater_.update();
}
 
void
GmclNode::initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
{
  handleInitialPoseMessage(*msg);
}
 
void
GmclNode::handleInitialPoseMessage(const geometry_msgs::PoseWithCovarianceStamped& msg)
{
  boost::recursive_mutex::scoped_lock prl(configuration_mutex_);
  if(msg.header.frame_id == "")
  {
    // This should be removed at some point
    ROS_WARN("Received initial pose with empty frame_id.  You should always supply a frame_id.");
  }
  // We only accept initial pose estimates in the global frame, #5148.
  else if(stripSlash(msg.header.frame_id) != global_frame_id_)
  {
    ROS_WARN("Ignoring initial pose in frame \"%s\"; initial poses must be in the global frame, \"%s\"",
             stripSlash(msg.header.frame_id).c_str(),
             global_frame_id_.c_str());
    return;
  }
 
  // In case the client sent us a pose estimate in the past, integrate the
  // intervening odometric change.
  geometry_msgs::TransformStamped tx_odom;
  try
  {
    ros::Time now = ros::Time::now();
    // wait a little for the latest tf to become available
    tx_odom = tf_->lookupTransform(base_frame_id_, msg.header.stamp,
                                   base_frame_id_, ros::Time::now(),
                                   odom_frame_id_, ros::Duration(0.5));
  }
  catch(tf2::TransformException e)
  {
    // If we've never sent a transform, then this is normal, because the
    // global_frame_id_ frame doesn't exist.  We only care about in-time
    // transformation for on-the-move pose-setting, so ignoring this
    // startup condition doesn't really cost us anything.
    if(sent_first_transform_)
      ROS_WARN("Failed to transform initial pose in time (%s)", e.what());
    tf2::convert(tf2::Transform::getIdentity(), tx_odom.transform);
  }
 
  tf2::Transform tx_odom_tf2;
  tf2::convert(tx_odom.transform, tx_odom_tf2);
  tf2::Transform pose_old, pose_new;
  tf2::convert(msg.pose.pose, pose_old);
  pose_new = pose_old * tx_odom_tf2;
 
  // Transform into the global frame
 
  ROS_INFO("Setting pose (%.6f): %.3f %.3f %.3f",
           ros::Time::now().toSec(),
           pose_new.getOrigin().x(),
           pose_new.getOrigin().y(),
           tf2::getYaw(pose_new.getRotation()));
  // Re-initialize the filter
  pf_vector_t pf_init_pose_mean = pf_vector_zero();
  pf_init_pose_mean.v[0] = pose_new.getOrigin().x();
  pf_init_pose_mean.v[1] = pose_new.getOrigin().y();
  pf_init_pose_mean.v[2] = tf2::getYaw(pose_new.getRotation());
  pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
  // Copy in the covariance, converting from 6-D to 3-D
  for(int i=0; i<2; i++)
  {
    for(int j=0; j<2; j++)
    {
      pf_init_pose_cov.m[i][j] = msg.pose.covariance[6*i+j];
    }
  }
  pf_init_pose_cov.m[2][2] = msg.pose.covariance[6*5+5];
 
  delete initial_pose_hyp_;
  initial_pose_hyp_ = new gmcl_hyp_t();
  initial_pose_hyp_->pf_pose_mean = pf_init_pose_mean;
  initial_pose_hyp_->pf_pose_cov = pf_init_pose_cov;
  applyInitialPose();
}
 
void
GmclNode::applyInitialPose()
{
  boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);
  if( initial_pose_hyp_ != NULL && map_ != NULL ) {
    pf_init(pf_, initial_pose_hyp_->pf_pose_mean, initial_pose_hyp_->pf_pose_cov);
    pf_init_ = false;
 
    delete initial_pose_hyp_;
    initial_pose_hyp_ = NULL;
  }
}
 
void
GmclNode::standardDeviationDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& diagnostic_status)
{
  double std_x = sqrt(last_published_pose.pose.covariance[6*0+0]);
  double std_y = sqrt(last_published_pose.pose.covariance[6*1+1]);
  double std_yaw = sqrt(last_published_pose.pose.covariance[6*5+5]);
 
  diagnostic_status.add("std_x", std_x);
  diagnostic_status.add("std_y", std_y);
  diagnostic_status.add("std_yaw", std_yaw);
  diagnostic_status.add("std_warn_level_x", std_warn_level_x_);
  diagnostic_status.add("std_warn_level_y", std_warn_level_y_);
  diagnostic_status.add("std_warn_level_yaw", std_warn_level_yaw_);
 
  if (std_x > std_warn_level_x_ || std_y > std_warn_level_y_ || std_yaw > std_warn_level_yaw_)
  {
    diagnostic_status.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Too large");
  }
  else
  {
    diagnostic_status.summary(diagnostic_msgs::DiagnosticStatus::OK, "OK");
  }
}