gmapping坐标系转换和基本数据结构

1、gmapping坐标系转换

        

1.1 map

        map坐标系是一个世界固定坐标系，其Z轴指向上方。相对于map坐标系的移动平台的姿态，不应该随时间显著移动。map坐标系作为长期的全局参考是很有用的，但是跳变使得对于本地传感和执行器来说，其实是一个不好的参考坐标。使用Gmapping建图时，建图起始点被设为map的原点（如下图）

 

1.2 base_link

        机器人本体坐标系，与机器人中心重合，也有的成为base_footprint。图中中心所有箭头指向的位置是base_link,机器人启动后静止时,base_link的左边是odom，也就是黄线的另一端（图1的map对应的黄线）：

 

1.3 odom

        里程计坐标系（odom或者odom_combined)，odom 坐标系是一个世界固定坐标系。在odom 坐标系中移动平台的位姿可以任意移动，没有任何界限。这种移动使得odom 坐标系不能作为长期的全局参考。然而，在odom 坐标系中的机器人的姿态能够保证是连续的，这意味着在odom 坐标系中的移动平台的姿态总是平滑变化，没有跳变。在一个典型设置中，odom 坐标系是基于测距源来计算的，如车轮里程计，视觉里程计或惯性测量单元。odom 坐标系作为一种精确，作为短期的本地参考是很有用的，但偏移使得它不能作为长期参考。

        如图，是机器人向前移动了一段距离的坐标系，可以看出odom与base_link之间的距离增大了一些，但是继续向前移动时，他们之间的距离在一定范围变动。因为odom是通过传感器测量，然后经过某些算法的计算得到的系统的估计值。

 

 1.4 自定义（传感器坐标系or其他）

        如图，laser_link激光雷达的坐标系，激光雷达的安装点，其与base_link的tf为固定的（安装在底盘上的）。

        wheel1_link,wheel2_link左右轮坐标系，同上：

        当机器人获得地图上的一个点（坐标系map）作为目标导航点，然后转换到odom坐标系下，传感器测量值读取转换到base_link(安装在机体上的器件与base_link存在固定的转换关系)，base_link转换到odom。

2、gmapping基本数据结构

2.1 Sensor_range

        在rangereading.h中，激光传感器数据类在rangereading.h中，这里重写了这个类，主要是在调用my_slam_gmapping.cpp/MySlamGMapping::addScan(const sensor_msgs::LaserScan& scan, GMapping::OrientedPoint& gmp_pose)，将每一帧激光数据，通过addScan函数封装成gmapping算法需要的数据格式。

代码如下：

#include <vector>
#include "../utils/point.h"

namespace GMapping
{

// 激光传感器数据类
class RangeReading
{
public:
    // 均匀角度的激光雷达数据
    // RangeReading(unsigned int n_beams, const double* d);

    // 不均匀角度的激光雷达数据
    RangeReading(unsigned int n_beams, const double* d, const double* angle);

    virtual ~RangeReading();

    // 得到这帧激光数据的激光雷达位姿
    inline const OrientedPoint& getPose() const
    {
        return m_pose;
    }
    // 设置这帧传感器数据的位姿
    inline void setPose(const OrientedPoint& pose)
    {
        m_pose = pose;
    }
    // 返回激光束的多少
    inline const unsigned int getSize() const
    {
        return m_beams;
    }

    // 存储激光雷达的距离信息
    std::vector<double> m_dists;
    unsigned int        m_beams;

    // 每个激光束对应的角度
    std::vector<double> m_angles;

protected:
    // 这帧激光数据的位姿
    OrientedPoint m_pose;
};

}

        rangereading.cpp代码如下，主要是一个构造函数，其中n_beams指定了激光束的数量：

// 不均匀角度的激光雷达数据
RangeReading::RangeReading(unsigned int n_beams, const double* d, const double* angle)
{
    m_beams = n_beams;
    m_dists.resize(n_beams);
    m_angles.resize(n_beams);
    for (unsigned int i=0; i<n_beams; i ++)
    {
        m_dists[i]  = d[i];
        m_angles[i] = angle[i];
    }
}

          

3、gmapping多线程操作

        首选包含头文件：

#include <boost/thread.hpp>

        比如my_slam_gmapping.cpp文件中的StartLiveSlam注册回调函数后，新启动了发布map到odom转换关系的线程：

/* 发布map到odom的转换关系的线程 */
transform_thread_ = new boost::thread(boost::bind(&MySlamGMapping::publishLoop, this, transform_publish_period_));

laserCallback函数中的：

// 多个线程访问同一资源时，为了保证数据的一致性，最简单的方式就是使用 mutex（互斥锁）
// 阻止了同一时刻有多个线程并发访问共享资源
map_to_odom_mutex_.lock();
map_to_odom_ = map_to_lidar * (odom_to_lidar.inverse()); // 表述里程计坐标系和map坐标系的差距
map_to_odom_mutex_.unlock();

        在后面的发布map和odom的转换关系，也就是StartLiveSlam调用的第三个函数中，密集使用map和odom的转换关系：

// StartLiveSlam函数调用的第③个函数：发布map->odom的转换关系
void MySlamGMapping::publishLoop(double transform_publish_period)
{
    if(transform_publish_period == 0)
        return;

    ros::Rate r(1.0 / transform_publish_period);

    while(ros::ok())
    {
        publishTransform();
        r.sleep();
    }
}
// 发布map到odom的转换关系
void MySlamGMapping::publishTransform()
{
    map_to_odom_mutex_.lock();
    // 默认情况下 tf_delay_ = transform_publish_period_;
    // 默认情况下ros::Duration(tf_delay_)时间长度，等于 r.sleep();的时间长度
    ros::Time tf_expiration = ros::Time::now() + ros::Duration(tf_delay_); // 这个没搞明白为啥要加这一点时间，感觉没有必要
    tfB_->sendTransform( tf::StampedTransform (map_to_odom_, tf_expiration, map_frame_, odom_frame_));
    map_to_odom_mutex_.unlock();
}