上一讲梳理了ExplorationPlanner文件夹下的grid文件夹下的所有头文件,这一讲我们梳理kinematics文件夹下的所有头文件。

ExplorationPlanner

ExplorationPlanner的头文件总览

从上面的截图可以看出,ExplorationPlanner分为这几部分:grid, kinematics, planner, sensor, trajectory, utils。这一讲主要是梳理kinematics部分。

2.kinematics部分

kinematics文件夹的头文件总览

1>IConstraint.h

先看引用的头文件,要使用智能指针std::unique_ptr,必须包含头文件<memory>。

#include <memory>

下面的这一段代码就是智能指针实现的地方;

virtual std::unique_ptr<IConstraint> clone() const = 0;

关于C++标准库提供的智能指针类std::unique_ptr,它是C++11新增的特性,具体补充:

接下来的代码,基本都加上了virtual关键字,为什么呢?是为了将这个类变成抽象基类。不能实例化的基类被称为抽象基类,这样的基类只有一个用途,那就是从它派生出其它类。在C++中,要创建抽象基类,可声明为纯虚函数。抽象基类提供了一种非常好的机制,让我们能够声明所有派生类都必须实现的函数。如果说Derived类从Base类派生而来,但是没有实现Derived::SomeFuncitionOfBase(),将无法通过编译。

virtual bool check (double value) const = 0;//纯虚函数
virtual double getMinValue() const = 0;//纯虚函数
virtual double getMaxValue() const = 0;//纯虚函数
virtual double forceToNearestLimit(double value) const = 0;//纯虚函数

那又为什么出现了克隆函数呢?这是因为C++不支持虚复制构造函数。如果能实现虚复制构造函数,则创建一个基类指针集合(如静态数组,其中的每个元素指向不同的派生类对象),并将其赋给另一个相同类型的数组时,虽然是通过基类指针调用的复制构造函数,但将复制指向的派生类对象,并将其进行深复制。

但是根本不可能实现虚复制构造函数,因为在基类方法声明中使用关键字vritual时,表示它将被派生类的实现覆盖,这种多态行为是在运行阶段实现的。而构造函数只能创建固定类型的对象,不具备多态性,因此C++不允许使用需虚复制构造函数。虽然如此,但存在一种解决方案,就是定义自己的克隆函数来实现上述目的。

虚函数Clone模拟了虚复制构造函数,但需要显式地调用。

//纯虚函数
//克隆函数
virtual std::unique_ptr<IConstraint> clone() const = 0;//虚函数clone模拟虚赋值构造函数

接下来的这一段代码中,添加vritual关键字,将析构函数声明为虚函数,用途:确保通过基类指针调用delete时,将调用派生类的析构函数。

为什么要这么做呢?这是因为对于使用new在自由存储区中实例化的派生类对象,如果将其赋给基类指针,并通过该指针调用delete,将不会调用派生类的析构函数。这可能导致资源未释放、内存泄漏等问题,因此为了避免这种问题,可将析构函数声明为虚函数。

virtual ~IConstraint() {} //虚析构函数

更通俗一点地讲,添加virtual关键字,这个细微的修改导致将运算符delete用于Base指针时,如果该指针向的是Derived对象,则编译器不仅会执行Base::~Base(),还会执行Derived::~Derived()。这将避免将delete用于Base指针时,派生类实例未被妥善销毁的情况发生。

类Base和Derived的虚函数表:

引自《Sams Teach Yourself C++ in One Hour a Day》

关于虚函数的工作原理--理解虚函数表,更多参考:

讲解完上述函数的C++语法,我们回到函数功能本身:

virtual bool check (double value) const = 0;//check功能
virtual double getMinValue() const = 0;//获取最小值
virtual double getMaxValue() const = 0;//获取最大值
//限制边界:forceToNearestLimit;输入一个double 型的value,返回一个double型;
virtual double forceToNearestLimit(double value) const = 0;

2>PlanarPose.h

这个头文件比较简单,整体功能就是用于获取x,y,theta;

double getX() const  {
        return x_;
 }
double getY() const  {
        return y_;
 }
double getTheta() const  {
        return theta_;
}

突出的C++语法就是构造函数,构造函数重载,复制构造函数了;

//构造函数;包含初始化列表
PlanarPose()
        : x_(0.0), y_(0.0), theta_(0.0)
    {
    }
//构造函数重载
 PlanarPose(double x, double y, double theta)
        : x_(x), y_(y), theta_(theta)
    {
    }

 //复制构造函数
 PlanarPose(const PlanarPose& p)
        : x_(p.x_), y_(p.y_), theta_(p.theta_)
    {

    }

构造函数是一种特殊的函数(方法),在根据类创建对象时被调用,它与类同名且不返回任何值,可在类声明中实现,也可在类声明外实现。与函数一样,构造函数也可以重载。

什么时候使用使用构造函数?构造函数总是在创建对象时被调用,这让构造函数称为类成员变量(int、指针等)初始化为选定值的理想场所。

至于什么是默认构造函数?可在不提供参数的情况下调用的构造函数被称为默认构造函数。默认构造函数是可选的。如果我们没有主动提供默认构造函数,编译器将为我们创建一个,这种构造函数会创建成员属性,但不会将POD类型(如int)的属性初始化为非零值。但是没有默认构造函数,而我们提供了重载的构造函数时,C++编译器不会为我们生成默认构造函数了。

关于包含初始化列表的构造函数,如下面这一段代码所示:

PlanarPose()
        : x_(0.0), y_(0.0), theta_(0.0)
    {
    }

我们知道构造函数对初始化成员很有用,另一种初始化成员的方式是使用初始化列表。

初始化列表由包含在括号中的参数声明后面的冒号表示,冒号后面列出了各个成员变量及其初始值。初始值可以是参数,也可以是固定值0.0,如上面x_(0.0),用 x_(x)表示都可。初始化列表还用另一个作用。

当基类包含重载的构造函数,需要在实例化时给它提供实参,创建派生对象时将怎么实例化这样的基类呢?方法是使用初始化列表,并通过派生类的构造函数调用合适的基类构造函数。

具体实例参考:

3>PlanarRobotVelCmd.h

经过上面关于构造函数语法的讲解,相信我们看到这段代码应该不发怵了:

PlanarRobotVelCmd()
        : linear_vel_mps_(0.0), angular_vel_rps_(0.0),
          final_rotation_rads(0.0), duration_secs_(0.0)
    {
    }

PlanarRobotVelCmd(double vel_lin, double vel_ang, double final_rot, double duration)
        : linear_vel_mps_(vel_lin), angular_vel_rps_(vel_ang),
          final_rotation_rads(final_rot), duration_secs_(duration)
  {
  }

再看到类里的私有成员:

double linear_vel_mps_;//表示线速度
double angular_vel_rps_;//表示角速度
double final_rotation_rads;//轨迹末端的旋转角度
double duration_secs_;//持续时间

再看其它函数的功能,作用就是获取线速度,角速度,轨迹末端的旋转角度以及持续时间:

double getLinearVelocity () const  {
        return linear_vel_mps_;
}
double getAngularVelocity ()  const {
        return angular_vel_rps_;
}
double getFinalRotation() const  {
        return final_rotation_rads;
}
double getDuration() const  {
        return duration_secs_;
}

4>IPlanarKinematics.h

先看引用的其它头文件,我们发现这正是我们上面提到的三个头文件:一个表示位姿x,y,theta,一个表示速度以及轨迹末端的旋转角度,最后一个是表示constraints的最小值与最大值。

#include "PlanarPose.h"
#include "PlanarRobotVelCmd.h"
#include "IConstraint.h"

再看到引用的其它C++头文件,其中看到有引用include <memory>,这意味着将要用到智能指针std::unique_ptr。

#include <vector>
#include <string>
#include <memory>

如下面的代码所示:

virtual std::unique_ptr<IPlanarKinematics> clone() const = 0;

纵观这个IPlanarKinematics.h所有的函数均加上了virtual关键字,那么我们可以判断这个IPlanarKinematics类是要被作为抽象基类了。

接下来专心讨论这个抽象基类具有的功能:

首先是checkConstraints,也就是验证约束,输入的是机器人的速度及轨迹末端的旋转角度、当前的位姿(x,y,theta),输出是一个bool型的真值

virtual bool checkConstraints(const PlanarRobotVelCmd& cmd,
                                  const PlanarPose& current_pose) const = 0;

然后是getNextPose,获取下一个位姿,输入的是当前位姿(x,y,theta)及速度和轨迹末端的旋转角度,输出是一个 PlanarPose类型的位姿。

virtual PlanarPose getNextPose(const PlanarPose& current_pose, const PlanarRobotVelCmd& cmd) const = 0;

紧接着,是getTrajectory ,获取轨迹,输入的是PlanarPose类型的vector容器表示的轨迹trajectory、PlanarPose类型的开始位姿(x,y,theta)、机器人的速度及轨迹末端的旋转角度;输出的话,前面表示的输入都加了引用&,这意味着更改后的值会相应地返回到这些加引用&的变量里。

virtual void getTrajectory(std::vector<PlanarPose>& trajectory,
                               const PlanarPose& start_pose,
                               const std::vector<PlanarRobotVelCmd>& commands) const = 0;

最后,是普普通通获取线速度、角速度、轨迹末端的旋转角度的对应函数:

virtual const IConstraint& getLinearVelC() const = 0;
virtual const IConstraint& getAngularVelC() const = 0;
virtual const IConstraint& getFinalRotC() const = 0;

5>VelocityPlanarKinematics.h

照例先看头文件,引用了kinematics目录下的两个头文件IPlanarKinematics.h、IConstraint.h,

#include "IPlanarKinematics.h"
#include "IConstraint.h

以及include <memory>用于使用智能指针std::unique_ptr,还用到了boost库的共享指针shared_ptr。

#include <memory>
#include <boost/shared_ptr.hpp>

boost库的shared_ptr参考链接:

下面的代码表示VelocityPlanarKinematics类继承了IPlanarKinematics类:

class VelocityPlanarKinematics : virtual public IPlanarKinematics

构造函数实现;

 VelocityPlanarKinematics( const IConstraint& vc,
                              const IConstraint& ac,
                              const IConstraint& fc);

接下来的是普普通通的继续添加virtual关键字的函数,这与IPlanarKinematics类的对应的函数形式差不多,只是函数末尾 没有 加"const = 0"格式,

 virtual bool checkConstraints(const PlanarRobotVelCmd& cmd,
                                  const PlanarPose& current_pose) const;
 virtual PlanarPose getNextPose(const PlanarPose& current_pose, const PlanarRobotVelCmd& cmd) const;
 virtual void getTrajectory(std::vector<PlanarPose>& trajectory, const PlanarPose& start_pose, const std::vector<PlanarRobotVelCmd>& commands) const;

然后是没有添加vritual关键字的函数,这部分函数与IPlanarKinematics类的函数形式也差不多,只不过没有添加virtual关键字:

const IConstraint& getLinearVelC() const
{
    return *linear_vel_c_;
}
const IConstraint& getAngularVelC() const
{
     return *angular_vel_c_;
}
const IConstraint& getFinalRotC() const
{
     return *final_rot_c_;
 }

其中这些返回的指针,在类的私有成员里这样定义:

private:
    std::unique_ptr<IConstraint> linear_vel_c_;
    std::unique_ptr<IConstraint> angular_vel_c_;
    std::unique_ptr<IConstraint> final_rot_c_;

6>RangeConstraint.h

看到头文件里,引用kinematics目录下的IConstraint.h;

#include "IConstraint.h"

我们再看到这个类RangeConstraint继承了IConstraint类;

class RangeConstraint : virtual public IConstraint

顺利继承了IConstraint类的所有函数,check,getMinValue,getMaxValue;

 bool check(double value) const;
 double getMinValue() const {
        return range_min_;
    }
 double getMaxValue() const {
        return range_max_;
    }

最后再将forceToNearestLimit函数补充里面的输入输出逻辑实现,实际上就是强制限制输入边界,将value的值限制在range_min和range_max_之间;

double forceToNearestLimit(double value) const
    {
        if (value < range_min_)
            return range_min_;
        else if ( value > range_max_)
            return range_max_;
        else
            return value;
    }

小结:

IConstraint.h使用virtual关键字将自己声明为抽象基类;包含的函数功能分别是:check(验证value),getMinValue(获取最小值),getMaxValue(获取最大值),forceToNearestLimit(限制输入的value在边界的最小值和最大值之间)。

PlanarPose.h这里用到了构造函数初始化列表,构造函数重载,复制构造函数;主要实现的功能是获取x,y,theta值;这个x,y,theta可以用来表示地图的原点origin,也可以表示机器人的当前位姿。

PlanarRobotVelCmd.h使用构造函数,构造函数重载,主要实现的功能是获取线速度,角速度,轨迹末端的旋转角度,持续时间。

IPlanarKinematics.h引用了上述三个头文件 IConstraint.h, PlanarPose.h, PlanarRobotVelCmd.h,并使用virtual关键字将自己声明为抽象基类;包含的函数功能分别是checkConstraints(验证是否在边界约束内),getNextPose(获取下一个位姿),getTrajectory(获取轨迹),getLinearVelC(获取线速度),getAngularVelC(获取角速度),getFinalRotC(获取轨迹末端的旋转角度)。

VelocityPlanarKinematics.h引用上述两个头文件IPlanarKinematics.h,IConstraint.h,

继承了抽象基类IPlanarKinematics类,因此都具备了抽象基类里的所有函数功能,只是将返回的线速度,角速度,轨迹末端的旋转角度都声明为IConstraint类型的智能指针,如std::unique_ptr<IConstraint> linear_vel_c;

RangeConstraint.h引用上述头文件IConstraint.h,继承了抽象基类IConstraint.h;因此也就具备了IConstraint类的所有函数功能,详情转至IConstraint.h;

综上所述,这6个头文件中有两个是抽象基类(一个用于限制value在边界内,另一个用于验证是否在边界约束内,获取下一个位姿,获取轨迹,获取速度和轨迹末端的旋转角度),还有两个分别是对应抽象基类的派生类,另外两个就是表示位姿、速度和轨迹末端的旋转角度;

接下来我们进入跟强化学习部分有关的头文件:IPlanarRobotVelCmdSampler.h, PlanarRobotVelCmdSamplerIndependentGaussian.h,PlanarRobotVelCmdSamplerUniform.h

1>IPlanarRobotVelCmdSampler.h

照例先看头文件,引用来自kinematic目录下的两个头文件:PlanarRobotVelCmd.h, IPlanarKinematics.h;

//实现的功能是获取线速度,角速度,轨迹末端的旋转角度,持续时间
#include "PlanarRobotVelCmd.h"
//用于验证是否在边界约束内,获取下一个位姿,获取轨迹,获取速度和轨迹末端的旋转角度
#include "IPlanarKinematics.h"

以及planner目录下的ExplorationTaskState.h

//设置机器人的位姿,获取机器人的位姿,获取占据栅格类型的地图,更新地图
#include "ExplorationPlanner/planner/ExplorationTaskState.h"

还有utils目录下的RNG.h

//用于产生随机数
#include "ExplorationPlanner/utils/RNG.h"

再回到IPlanarRobotVelCmdSampler类本身,这里使用virtual关键字将其表示为一个抽象基类,主要的功能是采样,主要功能是:

{上一次的速度,轨迹末端的旋转角度集合}中,再加上{验证是否在边界约束内,获取Next位姿、轨迹、线速度、角速度、轨迹末端的旋转角度}以及随机数输出{新的速度和轨迹末端的旋转角度集合}。

 virtual PlanarRobotVelCmd sample(const PlanarRobotVelCmd& old_cmd,
                                     const IPlanarKinematics& kinematics,
                                     RNG &rng) = 0;

2>PlanarRobotVelCmdSamplerUniform.h

从引用的头文件看,引用了kinematics目录下的两个头文件:

//从{上一次的速度,轨迹末端的旋转角度集合}中,
//再加上{验证是否在边界约束内,
//获取Next位姿、轨迹、线速度、角速度、轨迹末端的旋转角度}以及随机数,
//输出:{新的速度和轨迹末端的旋转角度集合}。
#include "IPlanarRobotVelCmdSampler.h"

/验证是否在边界约束内,获取下一个位姿,获取轨迹,获取线速度,获取角速度,
//获取轨迹末端的旋转角度
#include "VelocityPlanarKinematics.h"

以及utils目录下的RNG.h:

#include "ExplorationPlanner/utils/RNG.h"

另外还有boost库的uniform_real_distribution的hpp文件:

#include <boost/random/uniform_real_distribution.hpp>

关于boost库的uniform_real_distribution使用指南:

然后我们可以看到这个类PlanarRobotVelCmdSamplerUniform继承了抽象基类IPlanarRobotVelCmdSampler类;

class PlanarRobotVelCmdSamplerUniform : virtual public IPlanarRobotVelCmdSampler

除了继承了IPlanarRobotVelCmdSampler类的函数外,这个PlanarRobotVelCmdSamplerUniform类还实现了这个功能:

virtual std::unique_ptr<IPlanarRobotVelCmdSampler> clone() const
{
 return std::unique_ptr<IPlanarRobotVelCmdSampler>( new PlanarRobotVelCmdSamplerUniform(default_ctrl_duration_secs_));
}

此外在类的私有成员里额外声明了:

private:
   typedef boost::random::uniform_real_distribution<double> UniDist;
   double default_ctrl_duration_secs_;

3>PlanarRobotVelCmdSamplerIndependentGaussian.h

先看引用的头文件,有两个来自kinematics目录:

//验证是否在边界约束内,获取下一个位姿,获取轨迹,获取线速度,获取角速度,
//获取轨迹末端的旋转角度
#include "VelocityPlanarKinematics.h"

//从{上一次的速度,轨迹末端的旋转角度集合}中,
//再加上{验证是否在边界约束内,
//获取Next位姿、轨迹、线速度、角速度、轨迹末端的旋转角度}以及随机数,
//输出:{新的速度和轨迹末端的旋转角度集合}。
#include "IPlanarRobotVelCmdSampler.h"

以及utils目录下的RNG.h:

#include "ExplorationPlanner/utils/RNG.h"

另外还有一个boost库的normal_distribution的hpp文件:

#include <boost/random/normal_distribution.hpp>

关于boost库的normal_distribution使用指南:

然后我们可以看到这个类PlanarRobotVelCmdSamplerIndepent继承了抽象基类IPlanarRobotVelCmdSampler类;另外再实现自己独有的功能:

virtual std::unique_ptr<IPlanarRobotVelCmdSampler> clone() const
{
 return std::unique_ptr<IPlanarRobotVelCmdSampler>( new PlanarRobotVelCmdSamplerIndependentGaussian(lv_std_,
   av_std_,                                                                                                        fr_std_,
                                                                                                           default_ctrl_duration_secs_) );
}

在类的私有成员额外定义:

private:
    typedef boost::random::normal_distribution<double> NormalDist;
    double default_ctrl_duration_secs_;
    double lv_std_;//线速度
    double av_std_;//角速度
    double fr_std_;//轨迹末端的旋转角度

小结:

IPlanarRobotVelCmdSampler.h 实现的功能是从{上一次的速度,轨迹末端的旋转角度集合}中,再加上{验证是否在边界约束内,获取Next位姿、轨迹、线速度、角速度、轨迹末端的旋转角度}以及随机数输出{新的速度和轨迹末端的旋转角度集合},并将IPlanarRobotVelCmdSampler类 声明为抽象基类。

PlanarRobotVelCmdSamplerUniform.h 是抽象基类IPlanarRobotVelCmdSampler的派生类,继承了基类的所有功能,以及加入了自己的功能:生成均匀分布uniform_real_distribution。

PlanarRobotVelCmdSamplerIndependentGaussian.h是抽象基类IPlanarRobotVelCmdSampler的派生类,继承了基类的所有功能以及加入了自己的功能:生成正态分布normal_distribution。

综上所述,这3个头文件用于采样出新的速度和轨迹末端的旋转角度的集合,只不过可以选择使用均匀分布还是正态分布。如果对随机数的产生还不了解,请看附加链接:

总结:至此kinematics部分的头文件梳理完成,我们主要期待这部分的文件夹可以随机生成一系列的新的速度和轨迹末端的旋转角度集合。

Auf Wiedersehen!