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(ros::NodeHandle& nh, ros::NodeHandle& pnh);
00038     SlamGMapping(unsigned long int seed, unsigned long int max_duration_buffer);
00039     ~SlamGMapping();
00040 
00041     void init();
00042     void startLiveSlam();
00043     void startReplay(const std::string & bag_fname, std::string scan_topic);
00044     void publishTransform();
00045   
00046     void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan);
00047     bool mapCallback(nav_msgs::GetMap::Request  &req,
00048                      nav_msgs::GetMap::Response &res);
00049     void publishLoop(double transform_publish_period);
00050 
00051   private:
00052     ros::NodeHandle node_;
00053     ros::Publisher entropy_publisher_;
00054     ros::Publisher sst_;
00055     ros::Publisher sstm_;
00056     ros::ServiceServer ss_;
00057     tf::TransformListener tf_;
00058     message_filters::Subscriber<sensor_msgs::LaserScan>* scan_filter_sub_;
00059     tf::MessageFilter<sensor_msgs::LaserScan>* scan_filter_;
00060     tf::TransformBroadcaster* tfB_;
00061 
00062     GMapping::GridSlamProcessor* gsp_;
00063     GMapping::RangeSensor* gsp_laser_;
00064     // The angles in the laser, going from -x to x (adjustment is made to get the laser between
00065     // symmetrical bounds as that's what gmapping expects)
00066     std::vector<double> laser_angles_;
00067     // The pose, in the original laser frame, of the corresponding centered laser with z facing up
00068     tf::Stamped<tf::Pose> centered_laser_pose_;
00069     // Depending on the order of the elements in the scan and the orientation of the scan frame,
00070     // We might need to change the order of the scan
00071     bool do_reverse_range_;
00072     unsigned int gsp_laser_beam_count_;
00073     GMapping::OdometrySensor* gsp_odom_;
00074 
00075     bool got_first_scan_;
00076 
00077     bool got_map_;
00078     nav_msgs::GetMap::Response map_;
00079 
00080     ros::Duration map_update_interval_;
00081     tf::Transform map_to_odom_;
00082     boost::mutex map_to_odom_mutex_;
00083     boost::mutex map_mutex_;
00084 
00085     int laser_count_;
00086     int throttle_scans_;
00087 
00088     boost::thread* transform_thread_;
00089 
00090     std::string base_frame_;
00091     std::string laser_frame_;
00092     std::string map_frame_;
00093     std::string odom_frame_;
00094 
00095     void updateMap(const sensor_msgs::LaserScan& scan);
00096     bool getOdomPose(GMapping::OrientedPoint& gmap_pose, const ros::Time& t);
00097     bool initMapper(const sensor_msgs::LaserScan& scan);
00098     bool addScan(const sensor_msgs::LaserScan& scan, GMapping::OrientedPoint& gmap_pose);
00099     double computePoseEntropy();
00100     
00101     // Parameters used by GMapping
00102     double maxRange_;
00103     double maxUrange_;
00104     double maxrange_;
00105     double minimum_score_;
00106     double sigma_;
00107     int kernelSize_;
00108     double lstep_;
00109     double astep_;
00110     int iterations_;
00111     double lsigma_;
00112     double ogain_;
00113     int lskip_;
00114     double srr_;
00115     double srt_;
00116     double str_;
00117     double stt_;
00118     double linearUpdate_;
00119     double angularUpdate_;
00120     double temporalUpdate_;
00121     double resampleThreshold_;
00122     int particles_;
00123     double xmin_;
00124     double ymin_;
00125     double xmax_;
00126     double ymax_;
00127     double delta_;
00128     double occ_thresh_;
00129     double llsamplerange_;
00130     double llsamplestep_;
00131     double lasamplerange_;
00132     double lasamplestep_;
00133     
00134     ros::NodeHandle private_nh_;
00135     
00136     unsigned long int seed_;
00137     
00138     double transform_publish_period_;
00139     double tf_delay_;
00140 };


gmapping
Author(s): Brian Gerkey
autogenerated on Wed Aug 9 2017 02:37:49