(六) 源码分析4 processScan函数|激光SLAMgmapping

当前位姿与上一次位姿做差，计算做累计角度偏差和位移偏差。利用激光雷达测得距离做得分处理。非首帧调用scanMatch，upDateTreeWeight,resample。首帧则调用invalidActiveArea,computeActiveArea,registerScan。

第1部分

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
bool GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles)
{
    /* retireve the position from the reading, and compute the odometry */
    /*当前雷达在里程计坐标系的位置  addScan最后的setPose */
    OrientedPoint relPose = reading.getPose();

    /*m_count表示这个函数被调用的次数，开始为0,如果是第0次调用,则所有的位姿都是一样的*/
    if (!m_count)
	   m_lastPartPose = m_odoPose = relPose;
    
    /*对于每一个粒子，都从里程计运动模型中采样，得到车子的初步估计位置  这一步对应里程计的更新 */
    int tmp_size = m_particles.size();

    //这个for循环显然可以用 OpenMP 进行并行化
    for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++)
    { 
        OrientedPoint& pose(it->pose);      // 上一时刻粒子的位姿
        // relPose是里程计记录的最新位姿， m_odoPose是建图引擎记录的上一时刻的位姿
        // pose就是it->pose加了噪声，由于上面是引用，所以m_particles容器的成员pose全更新为pose
        pose=m_motionModel.drawFromMotion(it->pose, relPose, m_odoPose);
    }
    onOdometryUpdate();  /*回调函数，什么都没做，可以自己修改*/
    
    // 根据两次里程计的数据,计算机器人的线性位移和角度位移的累积值
    // m_odoPose表示上一次的里程计位姿   relPose表示新的里程计的位姿
    OrientedPoint move = relPose - m_odoPose;
    move.theta=atan2(sin(move.theta), cos(move.theta));

    // 统计机器人在进行激光雷达更新之前， 走了多远的距离  以及　平移了多少的角度
    // 这两个变量最后还是要清零
    m_linearDistance+=sqrt(move*move);   // x²+y²的开方
    m_angularDistance+=fabs(move.theta);

    // if the robot jumps throw a warning
    /*
     如果机器人在走了m_distanceThresholdCheck这么远的距离都没有进行激光雷达的更新
     则需要进行报警。这个误差很可能是里程计或者激光雷达的BUG造成的。
     例如里程计数据出错 或者 激光雷达很久没有数据等等
     每次进行激光雷达的更新之后 m_linearDistance这个参数就会清零
      m_distanceThresholdCheck在开头定义为5 */
    if (m_linearDistance > m_distanceThresholdCheck)
    {
			/*------- 一堆报警内容 -------*/
    }
    //更新:把当前的位置赋值给旧的位置
    m_odoPose = relPose;
     //先声明为false，最后如果成功就赋值 true
    bool processed=false;
    // 只有当机器人走过一定的距离或者旋转过一定的角度,或者过一段指定的时间才处理激光数据
    // 否则太低效了，period_被构造函数写死成5秒，可以考虑修改
    if ( ! m_count  ||   m_linearDistance >=m_linearThresholdDistance
            || m_angularDistance >=m_angularThresholdDistance
            || (period_ >= 0.0 && (reading.getTime() - last_update_time_) > period_) )
    {
   			/*第2部分*/
	}
    m_readingCount++;
    return processed;
}

ScanMatcher 构造函数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
ScanMatcher::ScanMatcher(): m_laserPose(0,0,0)
{
    line_angle = 0.0;
    m_laserBeams=0;
    m_optRecursiveIterations=9;
    m_activeAreaComputed=false;

    // 5cm解析度的默认参数
    // 这个参数是计算似然位姿的时候使用的，实际的gmapping中没有用到
    m_llsamplerange=0.01;
    m_llsamplestep=0.01;
    m_lasamplerange=0.005;
    m_lasamplestep=0.005;

    //地图进行拓展的大小
    m_enlargeStep=10.;
    m_fullnessThreshold=0.1;

    //指示里程计和陀螺仪是否可靠
    //如果可靠的话，那么进行score计算的时候，就需要对离里程计数据比较远的位姿增加惩罚
    //对于我们的应用来说，陀螺仪在短期内还是很可靠的。
    m_angularOdometryReliability=0.;
    m_linearOdometryReliability=0.;

    //理论上的离激光点的空闲距离 也就是说沿着激光束方向离激光点这么远距离的栅格一定是空闲的。
    m_freeCellRatio=sqrt(2.);

    //跳过一帧激光数据的开始几束激光
    m_initialBeamsSkip=0;   
    m_linePoints = new IntPoint[20000];
}

全是参数赋值，但是全写死，不能通过ROS修改

drawFromMotion 里程计运动模型

  • p 表示粒子估计的最优位置(机器人上一个时刻的最优位置)
  • pnew 表示里程计算出来的新的位置
  • pold 表示建图引擎记录的上一时刻的位姿(即上一个里程计的位置)

不是执行圆加运算，pnew和pold只是用于产生噪声控制量，然后加到p上

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
OrientedPoint 
MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const
{
    double sxy=0.3*srr;
     // 计算出pnew 相对于 pold走了多少距离，得到控制量
     // 这里的距离表达是相对于车身坐标系来说的
    OrientedPoint delta=absoluteDifference(pnew, pold);
    
    /*初始化一个点*/
    OrientedPoint noisypoint(delta);
    
    /*走过的三个方向的距离，为控制量添加上噪声项*/
    noisypoint.x+=sampleGaussian(srr*fabs(delta.x)+str*fabs(delta.theta)+sxy*fabs(delta.y));
    noisypoint.y+=sampleGaussian(srr*fabs(delta.y)+str*fabs(delta.theta)+sxy*fabs(delta.x));
    noisypoint.theta+=sampleGaussian(stt*fabs(delta.theta)+srt*sqrt(delta.x*delta.x+delta.y*delta.y));
    
    /*限制角度的范围为 -π～π  */
    noisypoint.theta=fmod(noisypoint.theta, 2*M_PI);
    if (noisypoint.theta>M_PI)
        noisypoint.theta-=2*M_PI;
    
    /*把加入了噪声的控制量 加到粒子估计的最优的位置上，得到新的粒子位姿的预估(根据运动模型推算)*/
    return absoluteSum(p,noisypoint);
}

这里的MotionModel是一个十分粗糙的运动模型，只是简单的矢量加减运算。 相比于《Probabilistic Robotics》中提到的速度模型和里程计模型而言，有很多方面都没有考虑，精度上可能有折扣。

第2部分 if中的处理激光数据

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
last_update_time_ = reading.getTime();
/*复制一帧数据 把激光数据转换为scan-match需要的格式*/
int beam_number = reading.getSize();
double * plainReading = new double[beam_number];
for(unsigned int i=0; i<beam_number; i++)
{
    plainReading[i]=reading.m_dists[i];
}
/*如果不是第一帧数据*/
if (m_count>0)
{
    /*
	  为每个粒子进行scanMatch，计算出来每个粒子的最优位姿，同时计算该最优位姿的得分和似然  对应于gmapping论文中的用最近的一次测量计算proposal的算法
	  除了进行scanMatch之外，还对粒子进行了权重的计算，并计算了粒子的有效区域，但不进行内存分配 内存分配在resample()函数中
	  这个函数在gridslamprocessor.hxx里
    */
    scanMatch(plainReading);

    //至此 关于proposal的更新完毕了，接下来是计算权重
    onScanmatchUpdate();
    /*
	  由于scanMatch中对粒子的权重进行了更新，那么这个时候各个粒子的轨迹上的累计权重都需要重新计算
	  这个函数即更新各个粒子的轨迹上的累计权重是更新
	  GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized) 函数在gridslamprocessor_tree.cpp里面实现
    */
    updateTreeWeights(false);
    /*
     粒子重采样  根据neff的大小来进行重采样  不但进行了重采样，也对地图进行更新
     GridSlamProcessor::resample 函数在gridslamprocessor.hxx里面实现
   */
    std::cerr<<"plainReading:"<<m_beams<<std::endl;
    resample(plainReading, adaptParticles, reading_copy);
}
/*如果是第一帧激光数据*/
else
{
    //如果是第一帧数据，则可以直接计算activeArea。因为这个时候，对机器人的位置是非常确定的，就是(0,0,0)
    for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++)
    {
        m_matcher.invalidateActiveArea();
        m_matcher.computeActiveArea(it->map, it->pose, plainReading);
        m_matcher.registerScan(it->map, it->pose, plainReading);

        //为每个粒子创建路径的第一个节点。该节点的权重为0,父节点为it->node(这个时候为NULL)。
        //因为第一个节点就是轨迹的根，所以没有父节点
        TNode* node=new	TNode(it->pose, 0., it->node,  0);
        node->reading = reading_copy;
        it->node=node;
    }
}
//	"Tree: normalizing, resetting and propagating weights at the end..." ;
//进行重采样之后，粒子的权重又会发生变化，因此需要再次更新粒子轨迹的累计权重
//GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized) 函数在gridslamprocessor_tree.cpp里面实现
updateTreeWeights(false);

delete [] plainReading;
m_lastPartPose=m_odoPose; //update the past pose for the next iteration

//机器人累计行走的多远的路程没有进行里程计的更新 每次更新完毕之后都要把这个数值清零
m_linearDistance=0;
m_angularDistance=0;

m_count++;
processed=true;

//keep ready for the next step
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++)
{
    it->previousPose=it->pose;
}
(四) 源码分析2 initMapper 第2部分|激光SLAMgmapping

initMapper 第2部分

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
// 激光的名称必须是"FLASER"
// 传入计算的angle increment绝对值, 因为gmapping接受正的angle increment
// 根据上面得到的激光雷达的数据信息， 初始化一个激光传感器
gsp_laser_ = new GMapping::RangeSensor("FLASER",
                                       gsp_laser_beam_count_,
                                       fabs(scan.angle_increment),
                                       gmap_pose,   // (0,0,0)
                                       0.0,
                                       maxRange_);
ROS_ASSERT(gsp_laser_);
// std::map<std::string, Sensor*>
GMapping::SensorMap  smap;
// getName()就是 FLASER
smap.insert(make_pair(gsp_laser_->getName(), gsp_laser_));
gsp_->setSensorMap(smap);   // GMapping::GridSlamProcessor*
// 构造函数里只有变量的赋值
gsp_odom_ = new GMapping::OdometrySensor(odom_frame_);
ROS_ASSERT(gsp_odom_);

///得到里程计的初始位姿，如果没有，则把初始位姿设置为(0,0,0)
GMapping::OrientedPoint  initialPose;
if(!getOdomPose(initialPose, scan.header.stamp))
{
  ROS_WARN("Unable to determine inital pose of laser! Starting point will be set to zero.");
  initialPose = GMapping::OrientedPoint(0.0, 0.0, 0.0);
}
// 以下全是参数的设置，纯粹变量赋值，不多不少
gsp_->setMatchingParameters(maxUrange_, maxRange_, sigma_,
                            kernelSize_, lstep_, astep_, iterations_,
                            lsigma_, ogain_, lskip_);

gsp_->setMotionModelParameters(srr_, srt_, str_, stt_);
gsp_->setUpdateDistances(linearUpdate_, angularUpdate_, resampleThreshold_);
gsp_->setUpdatePeriod(temporalUpdate_);
gsp_->setgenerateMap(false);
gsp_->GridSlamProcessor::init(particles_, xmin_, ymin_, xmax_, ymax_,
                              delta_, initialPose);
gsp_->setllsamplerange(llsamplerange_);
gsp_->setllsamplestep(llsamplestep_);
gsp_->setlasamplerange(lasamplerange_);
gsp_->setlasamplestep(lasamplestep_);
gsp_->setminimumScore(minimum_score_);

// 生成1作为方差，均值为0的高斯分布
GMapping::sampleGaussian(1, seed_);

ROS_INFO("Mapper Initialization complete");
return true;

getOdomPose

获得centered_laser_pose_ (laser坐标系的原点, 方向(0,0,angle_center)) 在odom坐标系中的坐标gmap_pose，即scan消息的时间戳时，激光雷达在里程计坐标系中的位姿

调用： getOdomPose(initialPose, scan.header.stamp)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
bool
SlamGMapping::getOdomPose(GMapping::OrientedPoint& gmap_pose, const ros::Time& t)
{
  // Get the pose of the centered laser at the right time
  centered_laser_pose_.stamp_ = t;

  tf::Stamped<tf::Transform> odom_pose;
  // 得到centered_laser_pose_ 在odom坐标系中的坐标 odom_pose
  try
  {
    tf_.transformPose(odom_frame_, centered_laser_pose_, odom_pose);
  }
  catch(tf::TransformException e)
  {
    ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
    return false;
  }
  double yaw = tf::getYaw(odom_pose.getRotation());
  ROS_INFO("get centered_laser_pose_'s odom pose: yaw: %f", yaw);
  ROS_INFO("centered_laser_pose_'s odom pose:: %f %f %f",odom_pose.getOrigin().x(), odom_pose.getOrigin().y(), odom_pose.getOrigin().z());
  gmap_pose = GMapping::OrientedPoint(odom_pose.getOrigin().x(),
                                      odom_pose.getOrigin().y(), yaw);
  return true;
}


sampleGaussian

S是随机数种子，保证每次的高斯分布不同。AMCL源码中是pf_pdf_gaussian_sample函数中调用pf_ran_gaussian

1
2
3
4
5
6
7
8
9
10
double sampleGaussian(double sigma, unsigned int S)
{
    if (S!=0)
    {
       srand(S);
    }
    if (sigma==0)
        return 0;
    return pf_ran_gaussian (sigma);
}


GridSlamProcessor::init

GridFastSLAM初始化主要是用来初始化各个粒子的一些信息。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
void GridSlamProcessor::init(unsigned int size, double xmin, double ymin, double xmax, double ymax, double delta, OrientedPoint initialPose)
{
  　　//设置地图大小和分辨率
   　m_xmin=xmin;
    m_ymin=ymin;
    m_xmax=xmax;
    m_ymax=ymax;
    m_delta=delta;
    
    /*设置每个粒子的初始值*/
    m_particles.clear();    // vector<Particle>
    TNode* node=new TNode(initialPose, 0, 0, 0);

    //粒子对应的地图进行初始化 用两个地图来进行初始化 一个高分辨率地图 一个低分辨率地图
    //高分辨率地图由自己指定 低分辨率地图固定为0.1m
    //定义最终使用的地图数据类型 cell类型为 PointAccumulator 存储数据类型为HierarchicalArray2D<PointAccumulator>
    // typedef Map<PointAccumulator,HierarchicalArray2D<PointAccumulator> > ScanMatcherMap;
    ScanMatcherMap lmap(Point(xmin+xmax, ymin+ymax)*.5, xmax-xmin, ymax-ymin, delta);
    ScanMatcherMap lowMap(Point(xmin+xmax,ymin+ymax)*0.5,xmax-xmin,ymax-ymin,　0.1);
    for (unsigned int i=0; i<size; i++)    // size是粒子数
    {
      m_particles.push_back(Particle(lmap,lowMap));
      //m_particles.push_back(Particle(lmap));
      m_particles.back().pose　=　initialPose;
      // 开始时，前一帧的粒子位姿也是 initialPose
      m_particles.back().previousPose　=　initialPose;
      m_particles.back().setWeight(0);
      m_particles.back().previousIndex=0;
      
      // we use the root directly
      m_particles.back().node= node;
    }
    m_neff=(double)size;
    m_count=0;
    m_readingCount=0;
    m_linearDistance=m_angularDistance=0;
  }

静态层|路径规划代价地图

静态层加载过程.png

静态层所用的参数：static map params

(五) 源码分析3 laserCallback第2部分及addScan, updateMap|激光SLAMgmapping

laserCallback 第2部分

接initMapper

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
GMapping::OrientedPoint odom_pose;
// addScan这个函数要转到pf的核心代码了，将调用processScan
if(addScan(*scan, odom_pose))
{
  ROS_DEBUG("scan processed done !");
  // 从粒子集合中挑选最优的粒子， 以其地图坐标为基准， 计算从激光雷达到地图之间的坐标变换
  GMapping::OrientedPoint mpose = gsp_->getParticles()[gsp_->getBestParticleIndex()].pose;
  ROS_INFO("new best pose: %.3f %.3f %.3f", mpose.x, mpose.y, mpose.theta);
  tf::Transform laser_to_map = tf::Transform(tf::createQuaternionFromRPY(0, 0, mpose.theta), tf::Vector3(mpose.x, mpose.y, 0.0)).inverse();

  ROS_INFO("odom pose: %.3f %.3f %.3f", odom_pose.x, odom_pose.y, odom_pose.theta);
  tf::Transform odom_to_laser = tf::Transform(tf::createQuaternionFromRPY(0, 0, odom_pose.theta), tf::Vector3(odom_pose.x, odom_pose.y, 0.0));

  ROS_WARN("correction: %.3f %.3f %.3f", mpose.x - odom_pose.x, mpose.y - odom_pose.y, mpose.theta - odom_pose.theta);

  // 另一处的lock在 SlamGMapping::publishTransform()， 这里是获得map_to_odom_，前者是发布
  map_to_odom_mutex_.lock();
  map_to_odom_ = (odom_to_laser * laser_to_map).inverse();
  map_to_odom_mutex_.unlock();
  // 如果没有地图则直接更新。 如果有地图了，两次扫描时间差大于参数，才更新地图
  if(!got_map_ || (scan->header.stamp - last_map_update) > map_update_interval_)
  {
    updateMap(*scan);
    last_map_update = scan->header.stamp;
    ROS_DEBUG("Updated the map");
  }
}
else
  ROS_WARN("cannot process scan");

这里如果addScan返回false，就是未向地图添加scan，原因在processScan里，可能是机器人走过的线距离太短或者旋转过角度太少，未达到门槛；或者未达到指定的时间(period_，写死为5秒)，毕竟我们不希望机器人停着不动就更新地图，那太消耗资源了。

updateMap()得到最优的粒子的地图数据，利用占据栅格地图算法，更新地图。

addScan

把新的测量值和里程计坐标报告给建图引擎gsp_，让其更新粒子集。 根据激光雷达的安装方式，对角度进行修改。

然后将ROS的激光雷达采集的信息转换成gmapping能看懂的格式。设置和激光数据时间戳匹配的机器人的位姿。调用processscan函数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
// 加入一个激光雷达的数据, 里面会调用 processScan()函数,这个函数被laserCallback()函数调用
// gmap_pose为刚定义的里程计的累积位姿
bool SlamGMapping::addScan(const sensor_msgs::LaserScan&  scan, GMapping::OrientedPoint&  gmap_pose)
{
    //得到与激光的时间戳相对应的机器人的里程计的位姿
    if(!getOdomPose(gmap_pose, scan.header.stamp))
        return false;

    //检测是否所有帧的数据都是相等的，如果不相等就return
    if(scan.ranges.size() != gsp_laser_beam_count_)
        return false;

    double* ranges_double = new double[scan.ranges.size()];

    // 如果激光是反着装的，这激光的顺序需要反过来，这段代码省略
    if (do_reverse_range_)
    {
        ......
    }
    else
    {
        for(unsigned int i=0; i < scan.ranges.size(); i++)
        {
            // mapper那里无法过滤掉
            // 排除掉所有激光距离小于range_min的值，它们很可能是噪声数据
            if(scan.ranges[i] < scan.range_min)
                ranges_double[i] = (double)scan.range_max;
            else
                ranges_double[i] = (double)scan.ranges[i];
        }
    }
    // 把ROS的激光雷达数据信息 转换为 GMapping算法看得懂的形式
    // 激光传感器gsp_laser_ 在initMapper里定义，ranges数据赋值给m_dists，size给m_beams
    GMapping::RangeReading reading(scan.ranges.size(),
                                   ranges_double,
                                   gsp_laser_,
                                   scan.header.stamp.toSec());
    // 上面的初始化是进行深拷贝，因此申请的内存可以直接释放。
    delete[] ranges_double;

    //设置和激光数据的时间戳匹配的机器人的位姿   不是雷达位姿吗???
    reading.setPose(gmap_pose);

    //调用gmapping算法进行处理
    return gsp_->processScan(reading);
}

gmap_pose应当是不同时间戳时，雷达扫描扇形的中心(包含角度)在odom坐标系中的坐标。

问题：

  • reading.setPose(gmap_pose); 解释存疑

最后就是核心算法所在的processScan

tf MessageFilter|ROSROS Kinetic知识

tf::MessageFilter可以订阅任何的ROS消息，然后将其缓存，直到这些消息可以转换到目标坐标系，然后进行相应的处理（进入回调函数）

tf::MessageFilter的初始化需要message_filters::Subscriber，tf转换，目标坐标系，等待时间。当message_filters::Subscriber的消息能够由tf转换到目标坐标系时，调用回调函数，回调函数由tf::MessageFilter::registerCallback()进行注册。

步骤:

  1. 定义数据：TransformListener、message_filters::Subscriber、tf::MessageFilter
  2. 用话题的名称来初始化message_filters::Subscriber
  3. 用tf message_filters::Subscriber 目标坐标系来初始化tf::MessageFilter
  4. 给tf::MessageFilter注册callback
    ，编写callback，并在回调中完成坐标转换，至此完成消息订阅+坐标转换
(三) 源码分析1 总体和laserCallback 第1部分|激光SLAMgmapping

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

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

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

1
2
3
4
5
6
7
8
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了：

1
2
3
4
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

1
2
3
4
5
6
7
8
9
10
11
12
//注册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_也很简单：

1
2
3
4
5
6
7
8
9
if(transform_publish_period == 0)
  return;

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


publishTransform如下：

1
2
3
4
5
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部分

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
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算法进行初始化
  1. 判断激光雷达是否是水平放置的，如果不是 则报错。
  2. 假设激光雷达数据的角度是对称的 & 递增的 为每个激光束分配角度。
  3. 为gmapping算法设置各种需要的参数。

initMapper 第1部分

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
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算法接受的点类型

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
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;
}

常用数据集|激光SLAM其他

我看到很多篇论文使用了log格式的数据集进行建图算法的仿真，比如这里的，发现这输入carmen框架，已经比较古老

  1. 进入informatik.uni-freiburg选择download log file下载数据集
  2. 看到一个文本网页,另存下载或者新建一個文件如data.clf保存.注意是.clf格式
  3. 这个clf文件显然不是我们想要的,可以转换为bag文件. 使用clf_to_bag.py


MIT data center的数据集页面
数据集2011-04-11-07-34-27.bag所用odom坐标系是odom_combined


俄勒冈大学的数据集

数据集hallway_slow_2011-03-04-21-41-33.bag所用的雷达话题是base_laser1_scan

参考：各类数据集

Matlab加载栅格地图|Matlab

实现二进制栅格地图

The object keeps track of three reference frames: world, local, and, grid. 世界坐标系的原点由GridLocationInWorld决定, which defines the bottom-left corner of the map relative to the world frame. LocalOriginInWorld property specifies the location of the origin of the local frame relative to the world frame. The first grid location with index (1,1) begins in the top-left corner of the grid.

map = binaryOccupancyMap creates a 2-D binary occupancy grid with a width and height of 10m. The default grid resolution is one cell per meter.

1
2
3
4
5
6
image = imread('F:\map.pgm');
%imshow(image)

imageBW = image < 100;
map = binaryOccupancyMap(imageBW);
show(map)

读取建好的pgm地图,如果直接imshow，结果就是pgm文件的本来样子

加下面几句后，结果是这样的
处理后的栅格地图.png

实现栅格地图

occupancyMap对象support local coordinates, world coordinates, and grid indices. The first grid location with index (1,1) begins in the top-left corner of the grid.

Use the occupancyMap class to create 2-D maps of an environment with probability values representing different obstacles in your world. You can specify exact probability values of cells or include observations from sensors such as laser scanners.

Probability values are stored using a binary Bayes filter to estimate the occupancy of each grid cell. A log-odds representation is used, with values stored as int16 to reduce the map storage size and allow for real-time applications.

1
2
3
4
5
image = imread('F:\map.pgm');
imageNorm = double(image)/255;
imageOccupancy = 1 - imageNorm;
map = occupancyMap(imageOccupancy,20);
show(map)

pgm文件中的值是0~255的uint8类型，将其归一化:先转为double类型,再除以255. 图片中的障碍物对应值为0,应该用1减去它,这样1就代表障碍物了. 否则图片显示出来是一团黑.

使用occupancyMap函数创建栅格地图,分辨率为1米20个cell,所支持的分辨率极限是±0.001

读取雷达扫描结果

1
2
3
4
5
6
7
8
9
10
11
12
13
14
# 建一个空地图,宽和高依次为10m,分辨率20,也就是1米有20个cell
map = occupancyMap(10,10,20);
# 机器人位姿[x,y,theta]
pose = [5,5,pi/2];
# ones函数是生成100行1列的矩阵, 元素全是1, 又都乘以3
# ones(n)函数是生成nXn的矩阵
ranges = 3*ones(100,1);
angles = linspace(-pi/2,pi/2,100);
maxrange = 20;
scan = lidarScan(ranges,angles);
# 把laser scan的数据插入栅格地图
insertRay(map,pose,scan,maxrange);

show(map)

linspace(x,y,n)是Matlab中的一个指令，用于产生x,y之间n点行矢量。其中x是起始值、y是中止值，n表示元素个数，如果缺省，则默认n为100。

linspace(1,10,2)为1，10. linspace(1,10,4)为1，4，7，10

laserScan对象

使用laserScan对象作为一些机器人算法的输入,例如matchScans, controllerVFH, monteCarloLocalization.

1
2
3
4
// ranges and angles inputs are vectors of the same length
scan = lidarScan(ranges,angles)
scan = lidarScan(cart)
scan = lidarScan(scanMsg) # 从ROS message中创建

plot(scan)可以显示雷达扫描的曲线

1
2
3
4
5
minRange = 0.1;
maxRange = 7;
scan2 = removeInvalidData(scan,'RangeLimits',[minRange maxRange]);
hold on
plot(scan2)

根据指定的范围,移除invalid数据

参考:Matlab occupancy map

全局路径规划(一) global_planner|路径规划move_base分析

概述

ROS 的navigation官方功能包提供了三种全局路径规划器：carrot_planner、global_planner、navfn，默认使用的是navfn。
继承关系 1.png

  • carrot_planner检查需要到达的目标是不是一个障碍物，如果是一个障碍物，它就将目标点替换成一个附近可接近的点。因此，这个模块其实并没有做任何全局规划的工作。在复杂的室内环境中，这个模块并不实用。

  • navfn使用Dijkstra算法找到最短路径。

  • global planner是navfn的升级版。它支持A*算法； 可以切换二次近似； 切换网格路径；

目前常用的是global_planner，需要先设定move_base的参数： base_global_planner: "global_planner/GlobalPlanner"。global_planner根据给定的目标位置进行总体路径的规划，只处理全局代价地图中的数据。提供快速的、内插值的全局规划，目前已经取代navfn。遵循navcore::navcore 包中指定的BaseGlobalPlanner接口。它接受costmap生成的全局代价地图规划出从起始点到目标点的路径，为local_planner规划路径作出参考。

global_planner没有提供类似D*这样的动态方法，而是用了定时规划路径，ROS是启动了一个线程，在移动过程中对路径不断的重新规划。这个feature是可以去掉的，特别是当你的运算负载很高，处理器又有限的情况下。还有重新规划(当找不到路径，也就是走着走着新扫描到未知区域的障碍或者动态增加的障碍)两种办法。加上了定时规划和重新规划之后的A*和D*几乎是一模一样的。

配置

move_base是通过plugin调用全局规划器的，文件bgp_plugin.xml

1
2
3
4
5
6
7
<library path="lib/libglobal_planner">
  <class name="global_planner/GlobalPlanner" type="global_planner::GlobalPlanner" base_class_type="nav_core::BaseGlobalPlanner">
    <description>
      A implementation of a grid based planner using Dijkstras or A*
    </description>
  </class>
</library>

在package.xml的配置中，加入如下行：
1
2
3
<export>
    <nav_core plugin="${prefix}/bgp_plugin.xml" />
</export>

参数

  • allow_unknown: true    是否允许规划器规划在未知区域创建规划，只设置该参数为true还不行,还要在costmap_commons_params.yaml中设置 track_unknown_space 参数也为true才行，

  • default_tolerance: 0.0    当设置的目的地被障碍物占据时，需要以该参数为半径寻找到最近的点作为新目的地点.

  • visualize_potential: false    是否显示从PointCloud2计算得到的势区域. 这个参数可以让你看见potential array的图像，看计算出的cost是怎么样子(颜色深浅代表距离起始点的远近)

  • use_dijkstra: true    设置为true,将使用dijkstra算法, 否则使用A*算法

  • use_quadratic: true    设置为true,将使用二次函数近似函数计算potential，否则使用更加简单的计算方式，这样节省硬件资源

  • use_grid_path: false    默认使用梯度下降法，路径更为光滑，从周围八个栅格中找到下降梯度最大的点。 如果为true，使用栅格路径，从终点开始找上下或左右4个中最小的栅格直到起点，会规划一条沿着网格边界的路径,偏向于直线穿越网格

  • old_navfn_behavior: false    navfn是非常旧的ROS系统中使用的，现在已经都用global_planner代替navfn了，所以不建议设置为true.

  • lethal_cost: 253    致命代价值,默认是设置为253,可以动态来配置该参数.

  • neutral_cost: 50    中等代价值,默认设置是50,可以动态配置该参数.

  • cost_factor: 3.0    代价地图与每个代价值相乘的因子.

  • publish_potential: true    是否发布costmap的势函数.

  • orientation_mode: 0    如何设置每个点的方向（None = 0,Forward = 1,Interpolate = 2,ForwardThenInterpolate = 3,Backward = 4,Leftward = 5,Rightward = 6）（可动态重新配置）

  • orientation_window_size: 1    根据orientation_mode指定的位置积分来得到使用窗口的方向.默认值1,可以动态重新配置.

       A*和Dijkstra两种算法

两种算法的效果对比
A.png
Dijkstra.png

A*比Dijkstra少计算很多，但可能不会产生相同路径。另外，在global_planner的A*里，the potentials are computed using 4-connected grid squares, while the path found by tracing the potential gradient from the goal back to the start uses the same grid in an 8-connected fashion. Thus, the actual path found may not be fully optimal in an 8-connected sense. (Also, no visited-state set is tracked while computing potentials, as in a more typical A* implementation, because such is unnecessary for 4-connected grids).

话题

发布的话题是~<name>/plan(nav_msgs/Path)，即最新规划出的路径，每次规划出新路径就要发布一次，主要用于观测。

GlobalPlanner::initialize

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
if (!initialized_)
{
    ros::NodeHandle private_nh("~/" + name);
    costmap_ = costmap;
    frame_id_ = frame_id;   //costmap_ros->getGlobalFrameID()

    unsigned int cx = costmap->getSizeInCellsX(), cy = costmap->getSizeInCellsY();
    // 参数赋值, 默认false
    private_nh.param("old_navfn_behavior", old_navfn_behavior_, false);
    if(!old_navfn_behavior_)
        convert_offset_ = 0.5;
    else
        convert_offset_ = 0.0;

    bool use_quadratic;   // 二次方的
    private_nh.param("use_quadratic", use_quadratic, true);

    // p_calc_声明为PotentialCalculator指针
    // QuadraticCalculator是它的派生类, 唯一区别是覆盖了calculatePotential函数
    if (use_quadratic)    // 默认使用二次
        p_calc_ = new QuadraticCalculator(cx, cy);
    else
        p_calc_ = new PotentialCalculator(cx, cy);

    bool use_dijkstra;
    private_nh.param("use_dijkstra", use_dijkstra, true);
    //DijkstraExpansion 和 AStarExpansion 都继承 Expander类
    if (use_dijkstra)
    {
        DijkstraExpansion* de = new DijkstraExpansion(p_calc_, cx, cy);
        if(!old_navfn_behavior_)
            de->setPreciseStart(true);
        planner_ = de;      // Expander* planner_;
    }
    else   // 如果不用dijkstra,就用A星算法.
        planner_ = new AStarExpansion(p_calc_, cx, cy);

    bool use_grid_path;
    private_nh.param("use_grid_path", use_grid_path, false);
    // GradientPath 和 GridPath都继承了Traceback
    if (use_grid_path)
        path_maker_ = new GridPath(p_calc_);
    else
        path_maker_ = new GradientPath(p_calc_);
        
    orientation_filter_ = new OrientationFilter();
    // NavfnROS::initialize中也注册此话题
    plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
    potential_pub_ = private_nh.advertise<nav_msgs::OccupancyGrid>("potential", 1);
    // 一系列参数赋值
    private_nh.param("allow_unknown", allow_unknown_, true);
    planner_->setHasUnknown(allow_unknown_);
    private_nh.param("planner_window_x", planner_window_x_, 0.0);
    private_nh.param("planner_window_y", planner_window_y_, 0.0);
    private_nh.param("default_tolerance", default_tolerance_, 0.0);
    private_nh.param("publish_scale", publish_scale_, 100);

    double costmap_pub_freq;
    private_nh.param("planner_costmap_publish_frequency", costmap_pub_freq, 0.0);

    //get the tf prefix
    ros::NodeHandle prefix_nh;
    tf_prefix_ = tf::getPrefixParam(prefix_nh);

    make_plan_srv_ = private_nh.advertiseService("make_plan", &GlobalPlanner::makePlanService, this);

    dsrv_ = new dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>(ros::NodeHandle("~/" + name));
    dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>::CallbackType cb = boost::bind(
            &GlobalPlanner::reconfigureCB, this, _1, _2);
    dsrv_->setCallback(cb);

    initialized_ = true;
} else
    ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing");

参考：
ROS- global_panner
global_planner历史背景和概述

Dijkstra算法|路径规划全局路径算法

以下面的加权图举例，说明算法的过程
加权图
加权图中不可有负权边
伪代码.png
或者这样理解：

  1. 第一个核心步骤：找到当前未处理过的顶点中 最小的点 V，（由于起点到起点的消耗为0，所以算法开始时 V 必定代表起点）；
  2. 第二个核心步骤：若V有邻居，则计算经过 V 的情况下起点到达各邻居的消耗 ，并选择是否更新 V 邻居的值。若没有邻居则对该点的处理结束
  3. 重复以上两个核心步骤，直到满足算法终止的条件：有向图中所有的点都被处理过。

算法每处理完一个顶点后，该顶点对应的 值就是起点到该点的最短路径长度，且在这之后不会被更改。这就是最短路径的原因
过程 1
过程 2

Dijkstra的特点是从起点开始，由近及远，层层扩展。越靠前处理的点离起点越近，最后一个处理的点离起点最远。
即使我只想找两个点之间的最短路径，Dijkstra还是需要遍历所有节点，因此时间复杂度为，所以不适合复杂路径等大规模场景。