gmapping: slam_gmapping.h Source File

00001 /*
00002  * slam_gmapping
00003  * Copyright (c) 2008, Willow Garage, Inc.
00004  *
00005  * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
00006  * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
00007  * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
00008  * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
00009  * 
00010  * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
00011  * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
00012  * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
00013  * CONDITIONS.
00014  *
00015  */
00016 
00017 /* Author: Brian Gerkey */
00018 
00019 #include "ros/ros.h"
00020 #include "sensor_msgs/LaserScan.h"
00021 #include "std_msgs/Float64.h"
00022 #include "nav_msgs/GetMap.h"
00023 #include "tf/transform_listener.h"
00024 #include "tf/transform_broadcaster.h"
00025 #include "message_filters/subscriber.h"
00026 #include "tf/message_filter.h"
00027 
00028 #include "gmapping/gridfastslam/gridslamprocessor.h"
00029 #include "gmapping/sensor/sensor_base/sensor.h"
00030 
00031 #include <boost/thread.hpp>
00032 
00033 class SlamGMapping
00034 {
00035   public:
00036     SlamGMapping();
00037     ~SlamGMapping();
00038 
00039     void publishTransform();
00040   
00041     void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan);
00042     bool mapCallback(nav_msgs::GetMap::Request  &req,
00043                      nav_msgs::GetMap::Response &res);
00044     void publishLoop(double transform_publish_period);
00045 
00046   private:
00047     ros::NodeHandle node_;
00048     ros::Publisher entropy_publisher_;
00049     ros::Publisher sst_;
00050     ros::Publisher sstm_;
00051     ros::ServiceServer ss_;
00052     tf::TransformListener tf_;
00053     message_filters::Subscriber<sensor_msgs::LaserScan>* scan_filter_sub_;
00054     tf::MessageFilter<sensor_msgs::LaserScan>* scan_filter_;
00055     tf::TransformBroadcaster* tfB_;
00056 
00057     GMapping::GridSlamProcessor* gsp_;
00058     GMapping::RangeSensor* gsp_laser_;
00059     double gsp_laser_angle_increment_;
00060     unsigned int gsp_laser_beam_count_;
00061     GMapping::OdometrySensor* gsp_odom_;
00062 
00063     bool got_first_scan_;
00064 
00065     bool got_map_;
00066     nav_msgs::GetMap::Response map_;
00067 
00068     ros::Duration map_update_interval_;
00069     tf::Transform map_to_odom_;
00070     boost::mutex map_to_odom_mutex_;
00071     boost::mutex map_mutex_;
00072 
00073     int laser_count_;
00074     int throttle_scans_;
00075 
00076     boost::thread* transform_thread_;
00077 
00078     std::string base_frame_;
00079     std::string laser_frame_;
00080     std::string map_frame_;
00081     std::string odom_frame_;
00082 
00083     void updateMap(const sensor_msgs::LaserScan& scan);
00084     bool getOdomPose(GMapping::OrientedPoint& gmap_pose, const ros::Time& t);
00085     bool initMapper(const sensor_msgs::LaserScan& scan);
00086     bool addScan(const sensor_msgs::LaserScan& scan, GMapping::OrientedPoint& gmap_pose);
00087     double computePoseEntropy();
00088     
00089     // Parameters used by GMapping
00090     double maxRange_;
00091     double maxUrange_;
00092     double maxrange_;
00093     double sigma_;
00094     int kernelSize_;
00095     double lstep_;
00096     double astep_;
00097     int iterations_;
00098     double lsigma_;
00099     double ogain_;
00100     int lskip_;
00101     double srr_;
00102     double srt_;
00103     double str_;
00104     double stt_;
00105     double linearUpdate_;
00106     double angularUpdate_;
00107     double temporalUpdate_;
00108     double resampleThreshold_;
00109     int particles_;
00110     double xmin_;
00111     double ymin_;
00112     double xmax_;
00113     double ymax_;
00114     double delta_;
00115     double occ_thresh_;
00116     double llsamplerange_;
00117     double llsamplestep_;
00118     double lasamplerange_;
00119     double lasamplestep_;
00120 };