slam_gmapping.h
Go to the documentation of this 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 };