10.gmapping建图拓展篇：ase_exploration包| 源码梳理(六)

Churlaaaaaaa：9.gmapping建图拓展篇：ase_exploration包| 源码梳理(五)

上一讲梳理了ExplorationPlanner文件夹下的sensor文件夹下的所有头文件，这一讲我们梳理trajectory文件夹下的所有头文件。

ExplorationPlanner

从上面的截图可以看出，ExplorationPlanner分为这几部分:grid, kinematics, planner, sensor, trajectory, utils。

5.trajectory部分

1>TrajectoryValue.h

老习惯了，先看引用的头文件，一个是kinematics目录下的PlanarPose.h，这里的PlanarPose类表示x,y,theta;另一个是sensor目录下的PlanarObservation.h，表示传感器模拟产生的observations。

#include "ExplorationPlanner/kinematics/PlanarPose.h"
#include "ExplorationPlanner/sensor/PlanarObservation.h

再回到TrajctoryValue类本身，先看它的构造函数，有两个参数:奖励reward_和观察observations_。

TrajectoryValue() :
        rewards_(),
        observations_()
 {
 }

类内的私有成员，reward_是用double型的vector表示，observations_是PlanarObservation类型的vector表示。

private:
    std::vector<double> rewards_;
    std::vector<PlanarObservation> observations_;

类内的函数功能实现，第一个函数getSumOfDiscountedRewards()，输入是折扣系数discount，函数的功能是获取乘以折扣系数后的reward的和。这里使用auto关键字，C++11语法新特性,功能是自动推断变量的类型，以及使用基于范围的for循环，这也是C++11新特性之一。

//获取乘以折扣系数后的reward的和
double getSumOfDiscountedRewards(double discount) const
    {
        double d = 1.0;
        double discounted_sum = 0.0;
        //使用auto关键字的基于范围的for循环
        for (auto r : rewards_)
        {
            discounted_sum += d * r;//等价于discount_sum = discount_sum + d*r;
            d *= discount;//等价于d = d*discount;
        }
        return discounted_sum;
    }

我们通过使用关键字auto来自动推断变量的类型，可编写一个通用的for循环，对任何类型的数组elements进行，从而进一步简化前面的for语句:

for(auto anElement : elements) //range based for
  cout << "Array elements are " << anElement << endl;

基于范围的for循环(Range-based for loop)具体例子实现参考:

Range-based for loop in C++ - GeeksforGeekswww.geeksforgeeks.org/range-based-loop-c/#:~:text=Range-based%20for%20loop%20in%20C%2B%2B%20is%20added%20since,values%2C%20such%20as%20all%20elements%20in%20a%20container

第二个函数使用push_back插入reward:

void insert(const double reward)
    {
        rewards_.push_back(reward);
    }

第三个函数使用push_back插入reward和observation，其实是第二个函数的重载实现:

void insert(const double reward, const PlanarObservation& observation)
    {
        rewards_.push_back(reward);
        observations_.push_back(observation);
    }

关于vector的push_back(unElement)指的是在vector最后一个元素后面插入新元素unElement；而pop_back()指的是删除vector最后一个元素。

vector::push_back() and vector::pop_back() in C++ STL - GeeksforGeekswww.geeksforgeeks.org/vectorpush_back-vectorpop_back-c-stl/#:~:text=vector%3A%3Apush_back%20%28%29%20push_back%20%28%29%20function%20is%20used%20to,and%20the%20container%20size%20is%20increased%20by%201

2>TrajectoryValueHandler.h

看头文件引用了TrajectoryValue.h，这个TrajectoryValue类用于将reward乘以折扣系数，再获得它们的和，以及实现reward和observation的插入。

#include "TrajectoryValue.h

回到TrajectoryValueHandler类本身，看它的私有成员变量:

private:
    double discount_;//折扣系数
    std::vector<TrajectoryValue> values_;//TrajectoryValue类型的vector集合

这里同样支持在这个TrajectoryValue类型的vector集合values_里增加同类型新的元素：

 void addValue(const TrajectoryValue& v)
    {
        values_.push_back( v );
    }

这里一个比较重要的函数是返回粒子的权重因子:

$new weight = old weight * weight factor;$

新的权重等于旧的权重乘以权重因子；

使用getWeightFactor()函数实现:

// Returns weight factor for the particle:
//  new weight = old weight * weight factor
double getWeightFactor(double subtract, double scaleFactor) const;

再来看TrajectoryValueHandler类的构造函数:

 TrajectoryValueHandler(double discount)
        : discount_(discount)
    {

    }

以及跟discount折扣系数有关的类内成员函数:

//获取最小的discountedreward和
double getMinSumOfDiscRew() const;
//获取最大的discountedreward和
double getMaxSumOfDiscRew() const;
//获取平均的discountedreward和
double getAvgSumOfDiscRew() const;

3>TrajectoryChecker.h

先看头文件引用了grid目录下的PlanarGridBinaryMap.h,这里面的PlanarGridBinaryMap类声明继承了能对栅格地图元素进行内容管理以及能在栅格地图中栅格单元遍历的管理工具；另外再加了自己一个独有的功能，由函数dilate()实现；

#include "ExplorationPlanner/grid/PlanarGridBinaryMap.h"

接下来是是kinematics目录下的PlanarPose.h,这个PlanarPose类的成员是x,y,theta，也用来表示机器人的位姿和地图的原点origin。

#include "ExplorationPlanner/kinematics/PlanarPose.h

剩下的就是引用boost库的std::shared_ptr:

#include <boost/shared_ptr.hpp>

boost库的shared_ptr:

shared_ptr - 1.61.0www.boost.org/doc/libs/1_61_0/libs/smart_ptr/shared_ptr.htm#:~:text=%EE%80%80shared_ptr%EE%80%81%20is%20now%20part%20of%20the%20C%2B%2B11%20Standard%2C,%5B%5D%20or%20T%20%5BN%5D%29%20as%20the%20template%20parameter

我们先来看TrajectoryChecker类里的私有成员，还未验证的栅格invalid_cells用一个PlanarGridBinaryMap类型的共享指针std::shared_ptr表示。

private:
   //PlanarGridBinaryMap类型的std_shared_ptr指针
 boost::shared_ptr<PlanarGridBinaryMap> invalid_cells_;
  //allow_unknow_cells是个关键参数
 bool allow_unknown_cells_;

再来看TrajectoryChecker类实现的功能:

首先是进行位姿验证isPoseValid()，传入参数是PlanarPose类型(x,y,theta)的元素;

bool isPoseValid(const PlanarPose& pose) const;

然后进行轨迹验证isTrajectoryValid(),传入参数是PlanarPose类型的vector,表示一系列位姿的集合：std::vector<PlanarPose>;

 bool isTrajectoryValid(const std::vector<PlanarPose>& poses) const;

紧接着就是栅格索引的验证isIndexValid()，传入参数是PlanarGridIndex类型(x,y)的元素;

 bool isIndexValid(const PlanarGridIndex& idx) const;

最后是根据距离获取栅格的数量getNumOfCellsWithinDistance()，传入参数是double型的距离；

unsigned int getNumOfCellsWithinDistance(double distance_meters) const;

4>TrajectoryGenerator.h

这个头文件名字直译过来就是轨迹生成器，我们从它引用的头文件可以看出绝大部分是来自kinematics目录下，目的是为了生成一系列位姿表示的轨迹以及速度及轨迹末端的采样集合。

//PlanarPose类表示x,y,theta；用来表示机器人的位姿或者地图原点origin都可
#include "ExplorationPlanner/kinematics/PlanarPose.h"
//PlanarRobotVelCmd类表示线速度，角速度，轨迹末端的最后旋转角度
#include "ExplorationPlanner/kinematics/PlanarRobotVelCmd.h"
//IPlanarRobotVelCmdSampler类表示采样生成速度和轨迹末端最后的旋转角度，是一个抽象基类
#include "ExplorationPlanner/kinematics/IPlanarRobotVelCmdSampler.h"
/*
IPlanarKinematics类表示验证约束(输入速度和当前位姿)，获取下一个位姿，获取轨迹，
获取线速度，获取角速度，或许轨迹末端的旋转角度
*/
#include "ExplorationPlanner/kinematics/IPlanarKinematics.h

还有一个来自trajectory本地目录，值的一提的是这个头文件里include了<memory>，而我们下面也用到了智能指针std::unique_ptr。而TrajectoryChecker类本身也实例化成一个checker，用于验证位姿，栅格索引以及轨迹。

#include "TrajectoryChecker.h"

最后一个引用是utils目录下的RNG.h，用于生成随机数:

#include "ExplorationPlanner/utils/RNG.h"

再看TrajectoryGenerator类的私有成员:

private:
//最短轨迹的长度
double minimum_trajectory_length_;
//最大的尝试次数
unsigned int max_tries_;

IPlanarKinematics类型的智能指针对象 kinematics_:

private:
 std::unique_ptr<IPlanarKinematics> kinematics_;

TrajectoryChecker类型的共享指针对象checker_:

private:
  boost::shared_ptr<TrajectoryChecker> checker_;

类内的私有函数:

getSubParts()，输入为部分轨迹，开始位姿，速度以及轨迹末端的旋转角度：

void getSubParts(std::vector<PlanarPose> &subparts,
                     const PlanarPose &start_pose,
                     const PlanarRobotVelCmd& command) const;

updatePoseAndCmd(),判断是否更新位姿和速度，输入为新的位姿，新的速度及轨迹末端的旋转角度，速度采样器，随机数，函数返回类型为bool型；

bool updatePoseAndCmd(PlanarPose& newpose,
                          PlanarRobotVelCmd& newcmd,
                          IPlanarRobotVelCmdSampler& sampler,
                          RNG& rng) const;

再看构造函数:

平平无奇的构造函数，输入是kinematics,checker,最短轨迹长度，最大尝试次数,值得一提是这里使用克隆函数clone()来完成对kinematics的参数的深拷贝；

TrajectoryGenerator(const IPlanarKinematics& kinematics,
                        const boost::shared_ptr<TrajectoryChecker>& checker,
                        double minimum_trajectory_length,
                        unsigned int max_tries)
        : kinematics_(kinematics.clone()),
          checker_(checker),
          minimum_trajectory_length_(minimum_trajectory_length),
          max_tries_(max_tries)
    {}

接下来就是复制构造函数，这并不感到奇怪，因为我们将会对轨迹对象做很多的操作:

//copy ctor
    TrajectoryGenerator(const TrajectoryGenerator& g)
        : kinematics_(g.kinematics_->clone()),
          checker_(g.checker_),
          minimum_trajectory_length_(g.minimum_trajectory_length_),
          max_tries_(g.max_tries_)
    {}

最后一个就是判断是否生成轨迹的函数，返回类型为bool型:

// Fill trajectory and commands
    bool generate(std::vector<PlanarPose>& trajectory,
                  std::vector<PlanarRobotVelCmd>& commands,
                  IPlanarRobotVelCmdSampler& sampler,
                  RNG& rng) const;

5>TrajectoryEvaluator.h

这里引用的头文件来自五个不同的目录:trajectory,kinematics,planner,sensor,utils。

可见这个TrajectoryEvaluator类是重量级别的类。

首先是trajectory本地目录下的TrajectoryValue.h用来验证位姿，栅格索引以及轨迹；

#include "TrajectoryValue.h

然后是kinematics目录下的PlanarPose.h用来表示位姿(x,y,theta)；

#include "ExplorationPlanner/kinematics/PlanarPose.h"

紧接着是planner目录下的ExplorationTaskState.h传入observations更新占据栅格地图，以及设置或者机器人位姿，获取占据栅格类型的地图；

#include "ExplorationPlanner/planner/ExplorationTaskState.h"

接下来是sensor目录下的LaserScanner2D.h表示一个激光传感器，返回observations，其中要对单个栅格单元的测量采样,将概率转换对数以及使用反演测量模型；

#include "ExplorationPlanner/sensor/LaserScanner2D.h"

接着是来自utils目录下的RNG.h，用来生成随机数;

#include "ExplorationPlanner/utils/RNG.h

最后一个比较陌生的，来自OpenMP，用来处理线程:

#include <omp.h>

关于OpenMP参考:

TeddyZhang：OpenMP并行开发（C++）104 赞同 · 10 评论文章

好了，回到这个TrajectoryEvaluator类本身，先看类的私有成员:

private:
    //这是一个激光传感器模型，每个线程都有它自己的传感器模型
    std::vector<LaserScanner2D> sensors_; // each thread has own sensor object

再来看TrajectoryEvaluator类同名的构造函数，这里传入的是一个LaserScanner2D类实例化的sensor对象；

TrajectoryEvaluator(const LaserScanner2D& sensor)
        : sensors_(std::max(1, omp_get_max_threads()), sensor)
    {

    }

最后是一个评价轨迹的函数evaluateTrajectory()，接受PlanarPose类型的位姿集合trajectory，PlanarGridOccupancy类型的占据栅格地图map 和RNG类型的随机数rng的传入。

其返回类型是TrajectoryValue类型，值得一提的是TrajectoryValue类成员包括奖励reward，观察observations，对奖励集合，观察集合中元素的插入，以及获取乘过折扣系数后的reward的和。

TrajectoryValue evaluateTrajectory(const std::vector<PlanarPose>& trajectory,
                  const PlanarGridOccupancyMap &map,
                  RNG& rng);

总结:

从上面的分析来看，我们可以感受明显的强化学习的影子:奖励reward, 折扣系数discount, 观察observation，速度指令velocity commands。

回顾上一讲，我们总结出由sensor,grid,kinematics三个文件夹组成了SLAM建图的部分。

首先是sensor文件下的激光传感器通过激光扫描并使用Bresenham画线算法来填充栅格单元，使用反演测量模型inverse sensor model，采样测量模型sample measurement model，根据激光扫描完成的observations完成栅格单元的更新，进一步完成整个栅格地图的更新，从而组成占据栅格地图算法构建的三个要素环：测量采样模型sample measurement model，更新栅格模型update occupancy grid，运动采样模型 sample motion model。

而这一讲，现在我们看到建图这部分仅仅是强化学习里的一个小小一环，占据栅格地图仅用于在评判轨迹，验证轨迹，这部分对应如我们分析ExplorationPlannerROS目录下的头文件源码中的costmap。

ExplorationPlannerROS源码解析的链接:

Churlaaaaaaa：5.gmapping建图拓展篇：ase_exploration包| 源码梳理(一)

而sensor文件夹则浓重地发挥了提供observations的功能。这正是强化学习中重要一环。

借助trajectory目录下的文件夹，生成轨迹，评判轨迹，验证轨迹，组成planner目录下粒子元素之一：轨迹trajectory。

planner目录下的粒子单元particle由权重weights，轨迹trajectory，速度及轨迹末端最后的旋转角度cmd组成。

其粒子的集合ParticleSet粒子集完整地经历粒子滤波的一切:

1.遍历粒子集的每个粒子

2.获取粒子数目

3.粒子权重赋值

4.获取拥有最大权重的粒子

5.获取整个粒子集的权重总和

6.粒子权重归一化

7.重采样

8.获取决定粒子重采样的 $Neff$ 值。

但是粒子particle也不是最终的boss，只是个小兵。planner目录下浓墨重彩描述的SMCPlanner才是强化学习足以与人类遥控操作探索建图的大boss。这才是论文强推的利器啊！

关于SMCPlanner还要跳回planner目录下:

Churlaaaaaaa：8.gmapping建图拓展篇：ase_exploration包| 源码梳理(四)

这里贴上planner篇的总结:

至于utils目录下的头文件，早已穿插讲过了。其实比较简单，就是关于栅格单元的熵值entropy,占据栅格用到的LogOdds，以及用于采样的随机数RNG。

Ok,撒花，完结！

Gracias a todos,jaja!

10.gmapping建图拓展篇：ase_exploration包| 源码梳理(六)

Churlaaaaaaa

ExplorationPlanner

5.trajectory部分

为你推荐

实际的机械臂控制（1）小强ROS机械臂Lobot的初步控制方法理解与实现第一节

小车yolo机械臂（六）ros gazebo 小车摄像头根据darknet_ros中yolo目标检测的信息进行自主运动

ros(26):ros::Time::now()，ros::Duration，toSec()，toNSec()；计算程序执行时间

Gazebo使用笔记（1） —— 简单建模操作

精选使用ROS与Movelt实现myCobot 280运动轨迹规划和控制

ROS学习（23）action通信机制

关于作者

Churlaaaaaaa

12

0

0

1

gmapping理论+对应ROS包讲解（三）ros 环境play 以及对应ros包说明

slam学习路线与心得总结

8.gmapping建图拓展篇：ase_exploration包| 源码梳理(四)

相关推荐

ROS学习（24）plugin插件

六轴协作机械臂人脸识别与跟踪！

cartographer代码流程分析

ROS中的坐标变换

机器人的ros_control硬件接口设计

ROS | 动作通信的编程实现

热门泡泡

30积分失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

10.gmapping建图拓展篇：ase_exploration包| 源码梳理(六)

Churlaaaaaaa

ExplorationPlanner

5.trajectory部分

为你推荐

实际的机械臂控制（1）小强ROS机械臂Lobot的初步控制方法理解与实现第一节

小车yolo机械臂（六）ros gazebo 小车摄像头根据darknet_ros中yolo目标检测的信息进行自主运动

ros(26):ros::Time::now()，ros::Duration，toSec()，toNSec()；计算程序执行时间

Gazebo使用笔记（1） —— 简单建模操作

精选使用ROS与Movelt实现myCobot 280运动轨迹规划和控制

ROS学习（23）action通信机制

评论（0）

关于作者

Churlaaaaaaa

12

0

0

1

gmapping理论+对应ROS包讲解（三）ros 环境play 以及对应ros包说明

slam学习路线与心得总结

8.gmapping建图拓展篇：ase_exploration包| 源码梳理(四)

相关推荐

ROS学习（24）plugin插件

六轴协作机械臂人脸识别与跟踪！

cartographer代码流程分析

ROS中的坐标变换

机器人的ros_control硬件接口设计

ROS | 动作通信的编程实现

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板