(三) 源码分析1 总体和laserCallback 第1部分

到gmapping包里，查看CMakeLists可知，gmapping所用的可执行文件主要是slam_gmapping，对应slam_gmapping.cpp。

用到了头文件gridslamprocessor.h，其位于/opt/ros/kinetic/include/gmapping/gridfastslam/

gmapping用到的很重要的动态库有/opt/ros/kinetic/lib/libgridfastslam.so，它的来源是：

add_library(gridfastslam
    gfsreader.cpp
    gridslamprocessor.cpp
    gridslamprocessor_tree.cpp
    motionmodel.cpp
)
# 又链接库 scanmatcher  sensor_range
target_link_libraries(gridfastslam scanmatcher sensor_range)

scanmatcher库的来源：
add_library(scanmatcher eig3.cpp scanmatcher.cpp scanmatcherprocessor.cpp smmap.cpp)

main函数如下，显然就看SlamGMapping了：

ros::init(argc, argv, "slam_gmapping");
SlamGMapping gn;
gn.startLiveSlam();
ros::spin();

gmapping流程图.png

构造函数

构造函数只有两行，定义了随机数种子，供之后的高斯采样使用。然后是init函数，它也很简单，先是下面两行，看来主要是类GridSlamProcessor

1 2	gsp_ = new GMapping::GridSlamProcessor(); tfB_ = new tf::TransformBroadcaster();

GridSlamProcessor构造函数其实有用的就一句period_ = 5.0;，其他是参数赋值，但在initMapper里又会重新赋值，其实是多余的。然后声明两个指针和布尔量，剩下的代码全是读取和配置参数了

startLiveSlam

//注册entryopy  map  map_metadata话题　　　dynamic_map服务
entropy_publisher_ = private_nh_.advertise<std_msgs::Float64>("entropy", 1, true);
sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);

scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);
scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));

// 以成员函数publishLoop作为线程启动，参数为　transform_publish_period,默认　0.05
transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));

mapCallback是service的回调函数，留给使用者定义客户端，这样一来重点是laserCallback，稍后再分析。

线程transform_thread_也很简单：

if(transform_publish_period == 0)
  return;

ros::Rate r(1.0 / transform_publish_period);
while(ros::ok())
{
  publishTransform();
  r.sleep();
}

publishTransform如下：

map_to_odom_mutex_.lock();
ros::Time tf_expiration = ros::Time::now() + ros::Duration(tf_delay_);
// 构造函数里定义的广播，　发布map和odom之间的变换
tfB_->sendTransform( tf::StampedTransform (map_to_odom_, tf_expiration, map_frame_, odom_frame_));
map_to_odom_mutex_.unlock();

laserCallback 第1部分

laser_count_++;
/*每隔throttle_scans_ （默认值 1）帧数据计算一次，限流作用*/
if ((laser_count_ % throttle_scans_) != 0)
  return;

static ros::Time last_map_update(0,0);

if(!got_first_scan_)
{
  /*重要参数的初始化，将slam里的参数传递到 openslam 里 ，设定坐标系，坐标原点，以及
  采样函数随机种子的初始化，还调用了GridSlamProcessor::init,初始化了粒子数，子地图大小*/
  if(!initMapper(*scan))
      return;
  got_first_scan_ = true;
}

下面分析函数 bool SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)

这个函数在收到的第一帧激光雷达数据的时候会被调用一次，之后就再也不会被调用了。
这个函数的功能主要就是对gmapping算法中需要的一些参数进行赋值，即对gmapping算法进行初始化

判断激光雷达是否是水平放置的，如果不是则报错。
假设激光雷达数据的角度是对称的 & 递增的为每个激光束分配角度。
为gmapping算法设置各种需要的参数。

initMapper 第1部分

laser_frame_ = scan.header.frame_id;
// 雷达坐标系原点在base_link中的位置
tf::Stamped<tf::Pose> ident;
tf::Stamped<tf::Transform> laser_pose;
ident.setIdentity();
ident.frame_id_ = laser_frame_;   // "laser"
ident.stamp_ = scan.header.stamp;
ROS_INFO("ident : %f %f %f",ident.getOrigin().x(), ident.getOrigin().y(), ident.getOrigin().z());
try
{
  // 输出laser_pose，雷达坐标系原点在base_link中的位置
  tf_.transformPose(base_frame_, ident, laser_pose);
}
catch(tf::TransformException e)
{
  ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
           e.what());
  return false;
}
ROS_INFO("laser_pose: %f %f %f",laser_pose.getOrigin().x(), laser_pose.getOrigin().y(), laser_pose.getOrigin().z());
// base_link坐标系中，创建laser_pose１米之上的点，再把它转到laser坐标系
tf::Vector3 v;
v.setValue(0, 0, 1 + laser_pose.getOrigin().z());
tf::Stamped<tf::Vector3> up(v, scan.header.stamp, base_frame_);
try
{
  tf_.transformPoint(laser_frame_, up, up);
  ROS_DEBUG("Z-Axis in sensor frame: %.3f", up.z());
}
catch(tf::TransformException& e)
{
  ROS_WARN("Unable to determine orientation of laser: %s",
           e.what());
  return false;
}

// gmapping doesn't take roll or pitch into account.
// 这里是判断雷达是否倾斜
if (fabs(fabs(up.z()) - 1) > 0.001)
{
  ROS_WARN("Laser has to be mounted planar! Z-coordinate has to be 1 or -1, but gave: %.5f",
               up.z());
  return false;
}
gsp_laser_beam_count_ = scan.ranges.size();
// 扇形扫描面的中心
double angle_center = (scan.angle_min + scan.angle_max)/2;
// 雷达是否正装
if (up.z() > 0)
{
  do_reverse_range_ = scan.angle_min > scan.angle_max;
  // laser坐标系中的点(0,0,0), 方向(0,0,angle_center)
  centered_laser_pose_ = tf::Stamped<tf::Pose>(
    tf::Transform(tf::createQuaternionFromRPY(0,0,angle_center), 
      tf::Vector3(0,0,0)),  ros::Time::now(),  laser_frame_);
  ROS_INFO("Laser is mounted upwards.");
}
else
{
  do_reverse_range_ = scan.angle_min < scan.angle_max;
  centered_laser_pose_ = tf::Stamped<tf::Pose>(tf::Transform(tf::createQuaternionFromRPY(M_PI,0,-angle_center),
                                                             tf::Vector3(0,0,0)), ros::Time::now(), laser_frame_);
  ROS_INFO("Laser is mounted upside down.");
}
// Compute the angles of the laser from -x to x, basically symmetric and in increasing order
laser_angles_.resize(scan.ranges.size());
// 把角度调成对称，也就是扇形要关于ｚ轴对称，扫描角度从-3.124～3.142变为-3.133～3.133
double theta = - std::fabs(scan.angle_min - scan.angle_max)/2;
for(unsigned int i=0; i<scan.ranges.size(); ++i)
{
  laser_angles_[i]=theta;
  theta += std::fabs(scan.angle_increment);
}

ROS_INFO("Laser angles in laser-frame: min: %.3f max: %.3f inc: %.3f", scan.angle_min, scan.angle_max,
          scan.angle_increment);
ROS_INFO("Laser angles in top-down centered laser-frame: min: %.3f max: %.3f inc: %.3f", laser_angles_.front(),  laser_angles_.back(), std::fabs(scan.angle_increment));

// 这个是Gmapping算法接受的点类型
GMapping::OrientedPoint gmap_pose(0, 0, 0);
// 设置激光雷达的最大观测距离和最大使用距离
ros::NodeHandle private_nh_("~");
if(!private_nh_.getParam("maxRange", maxRange_))
    maxRange_ = scan.range_max-0.01;
if(!private_nh_.getParam("maxUrange", maxUrange_))
    maxUrange_=maxRange_;

GMapping::OrientedPoint

这个是Gmapping算法接受的点类型

template <class T, class A>
struct orientedpoint: public point<T>{
  inline orientedpoint() : point<T>(0,0), theta(0) {};
    inline orientedpoint(const point<T>& p);
    inline orientedpoint(T x, T y, A _theta): point<T>(x,y), theta(_theta){}
        inline void normalize();
    inline orientedpoint<T,A> rotate(A alpha)
    {
        T s=sin(alpha), c=cos(alpha);
        A a=alpha+theta;
        a=atan2(sin(a),cos(a));
        return orientedpoint(
            c*this->x-s*this->y,
            s*this->x+c*this->y, 
            a);
    }
    A theta;
};

template <class T, class A>
void orientedpoint<T,A>::normalize()
{
  if (theta >= -M_PI && theta < M_PI)
    return;
  
  int multiplier = (int)(theta / (2*M_PI));
  theta = theta - multiplier*2*M_PI;
  if (theta >= M_PI)
    theta -= 2*M_PI;
  if (theta < -M_PI)
    theta += 2*M_PI;
}