Fawkes API  Fawkes Development Version
amcl_thread.h
00001 /***************************************************************************
00002  *  amcl_thread.cpp - Thread to perform localization
00003  *
00004  *  Created: Wed May 16 16:03:38 2012
00005  *  Copyright  2012  Tim Niemueller [www.niemueller.de]
00006  ****************************************************************************/
00007 
00008 /*  This program is free software; you can redistribute it and/or modify
00009  *  it under the terms of the GNU General Public License as published by
00010  *  the Free Software Foundation; either version 2 of the License, or
00011  *  (at your option) any later version.
00012  *
00013  *  This program is distributed in the hope that it will be useful,
00014  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00015  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00016  *  GNU Library General Public License for more details.
00017  *
00018  *  Read the full text in the LICENSE.GPL file in the doc directory.
00019  */
00020 
00021 #ifndef __PLUGINS_AMCL_AMCL_THREAD_H_
00022 #define __PLUGINS_AMCL_AMCL_THREAD_H_
00023 
00024 #define NEW_UNIFORM_SAMPLING 1
00025 
00026 #include "map/map.h"
00027 #include "pf/pf.h"
00028 #include "pf/pf_vector.h"
00029 #include "sensors/amcl_odom.h"
00030 #include "sensors/amcl_laser.h"
00031 
00032 #include <core/threading/thread.h>
00033 #include <aspect/blocked_timing.h>
00034 #include <aspect/clock.h>
00035 #include <aspect/configurable.h>
00036 #include <aspect/logging.h>
00037 #include <aspect/tf.h>
00038 #include <aspect/blackboard.h>
00039 
00040 #include <interfaces/Laser360Interface.h>
00041 #include <interfaces/Position3DInterface.h>
00042 
00043 #include <algorithm>
00044 #include <vector>
00045 #include <map>
00046 #include <cmath>
00047 
00048 #ifdef HAVE_ROS
00049 #  include <plugins/ros/aspect/ros.h>
00050 #  include <ros/publisher.h>
00051 #  include <ros/subscriber.h>
00052 #  include <geometry_msgs/PoseWithCovarianceStamped.h>
00053 #endif
00054 
00055 
00056 /// Pose hypothesis
00057 typedef struct {
00058   /// Total weight (weights sum to 1)
00059   double weight;
00060   /// Mean of pose esimate
00061   pf_vector_t pf_pose_mean;
00062   /// Covariance of pose estimate
00063   pf_matrix_t pf_pose_cov;
00064 } amcl_hyp_t;
00065 
00066 namespace fawkes {
00067   class Mutex;
00068 }
00069 
00070 class AmclThread
00071 : public fawkes::Thread,
00072   public fawkes::ClockAspect,
00073   public fawkes::LoggingAspect,
00074   public fawkes::ConfigurableAspect,
00075   public fawkes::BlockedTimingAspect,
00076   public fawkes::BlackBoardAspect,
00077 #ifdef HAVE_ROS
00078   public fawkes::ROSAspect,
00079 #endif
00080   public fawkes::TransformAspect
00081 {
00082 public:
00083   AmclThread();
00084   virtual ~AmclThread();
00085 
00086   virtual void init();
00087   virtual void loop();
00088   virtual void finalize();
00089   /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
00090  protected: virtual void run() { Thread::run();}
00091 
00092  private:
00093   bool set_laser_pose();
00094   bool get_odom_pose(fawkes::tf::Stamped<fawkes::tf::Pose>& odom_pose,
00095                      double& x, double& y, double& yaw,
00096                      const fawkes::Time* t, const std::string& f);
00097   void apply_initial_pose();
00098   static pf_vector_t uniform_pose_generator(void* arg);
00099 #ifdef HAVE_ROS
00100   void initial_pose_received(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg);
00101 #  ifdef USE_MAP_PUB
00102   void publish_map();
00103 #  endif
00104 #endif
00105 
00106 private:
00107   fawkes::Mutex *conf_mutex_;
00108 
00109   std::string  cfg_map_file_;
00110   float        cfg_resolution_;
00111   float        cfg_origin_x_;
00112   float        cfg_origin_y_;
00113   float        cfg_origin_theta_;
00114   float        cfg_occupied_thresh_;
00115   float        cfg_free_thresh_;
00116 
00117   std::string cfg_laser_ifname_;
00118   std::string cfg_pose_ifname_;
00119 
00120   unsigned int map_width_;
00121   unsigned int map_height_;
00122   bool laser_pose_set_;
00123 
00124   fawkes::tf::Transform latest_tf_;
00125 
00126   amcl::odom_model_t  odom_model_type_;
00127   amcl::laser_model_t laser_model_type_;
00128 
00129   int max_beams_, min_particles_, max_particles_;
00130 
00131   bool sent_first_transform_;
00132   bool latest_tf_valid_;
00133   map_t* map_;
00134   pf_t *pf_;
00135   int resample_count_;
00136 
00137   double gui_publish_period;
00138   double save_pose_period;
00139   double cloud_pub_interval;
00140   double transform_tolerance_;
00141   fawkes::Time save_pose_last_time;
00142 
00143   fawkes::Laser360Interface* laser_if_;
00144   fawkes::Position3DInterface * pos3d_if_;
00145 
00146 #ifdef HAVE_ROS
00147   ros::Publisher pose_pub_;
00148   ros::Publisher particlecloud_pub_;
00149   ros::Subscriber initial_pose_sub_;
00150   ros::Publisher map_pub_;
00151 #endif
00152 
00153   amcl_hyp_t* initial_pose_hyp_;
00154   bool first_map_received_;
00155   bool first_reconfigure_call_;
00156 
00157   char* mapdata;
00158   int sx, sy;
00159   double resolution;
00160 
00161 
00162   // Particle filter
00163   double pf_err_, pf_z_;
00164   bool pf_init_;
00165   pf_vector_t pf_odom_pose_;
00166   double laser_min_range_;
00167   double laser_max_range_;
00168 
00169   amcl::AMCLOdom* odom_;
00170   amcl::AMCLLaser* laser_;
00171   bool laser_update_;
00172 
00173   fawkes::Time last_cloud_pub_time;
00174   fawkes::Time last_laser_received_ts_;
00175   double last_covariance_[36];
00176 
00177 
00178   float alpha1_;
00179   float alpha2_;
00180   float alpha3_;
00181   float alpha4_;
00182   float alpha5_;
00183   float z_hit_;
00184   float z_short_;
00185   float z_max_;
00186   float z_rand_;
00187   float sigma_hit_;
00188   float lambda_short_;
00189   float laser_likelihood_max_dist_;
00190   float d_thresh_;
00191   float a_thresh_;
00192   float t_thresh_;
00193   float alpha_slow_;
00194   float alpha_fast_;
00195   float init_pose_[3];
00196   float init_cov_[3];
00197   float angle_increment_;
00198   float angle_min_;
00199   unsigned int angle_min_idx_;
00200   unsigned int angle_max_idx_;
00201   unsigned int angle_range_;
00202 
00203   unsigned int resample_interval_;
00204 
00205   fawkes::Time *last_move_time_;
00206 
00207   std::string odom_frame_id_;
00208   std::string base_frame_id_;
00209   std::string global_frame_id_;
00210   std::string laser_frame_id_;
00211 
00212 #if NEW_UNIFORM_SAMPLING
00213     static std::vector<std::pair<int,int> > free_space_indices;
00214 #endif
00215 };
00216 
00217 #endif