文章目录

1. 设置机器人的形状

RobotFootprintModel为机器人的模型基类，用来定义机器人的形状。但机器人模型类目前只用于优化，因为考虑到导航栈的足迹可能是低效的。因此，设置机器人的形状只是用来检测可行性。

此类族在robot_footprint_model.h头文件中。

1.1 BaseRobotFootprintModel

函数如下：

calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) 计算机器人到障碍物的距离。其中current_pose为当前机器人的位姿。
estimateSpatioTemporalDistance(const PoseSE2& current_pose, const Obstacle* obstacle, double t) 计算机器人到障碍物的时空距离。其中current_pose为当前机器人的位姿。
getInscribedRadius() 计算机器人的半径

1.2 其他RobotFootprintModel

下面介绍更具体的模型。

PointRobotFootprint 点机器人足迹模型。不使用CircularRobotFootprint，而是使用这个类，机器人的半径可以被添加到最小距离参数中。这就避免了每次计算距离时都要减去零。
CircularRobotFootprint 圆形机器人足迹模型。建议用上面的点足迹模型，可以优化计算。
TwoCirclesRobotFootprint 双圆形机器人足迹模型。用两个移位的圆来近似机器人。
LineRobotFootprint 线段机器人足迹模型。这个模型中宽度为0。
PolygonRobotFootprint 多边形机器人足迹模型。这个模型用来表示足迹为封闭多边形的小车。

2. TEB路线规划器

teb_local_planner是TEB算法的核心代码。里面包含了对最优路径的选择和计算，并且，可以求出当前应该要行驶的线速度和角速度。

2.1 基类PlannerInterface

PlannerInterface是TEB算法继承的基类。里面定义了一个规划器应该具有的功能和能提供的服务。

里面的函数有：

plan: 用于规划路径的
getVelocityCommand: 根据上一次的路径规划获取速度指令的
clearPlanner(): 重置规划器清除之前记录的规划结果的
setPreferredTurningDir(RotType dir): 选择于一个理想的初始转弯方向（通过惩罚反对的方向），如果计划的轨迹在两个成本相近的解决方案（在同一等价类中！）之间摇摆，可以指定一个期望的（初始）转弯方向。检查参数，以便调整惩罚的权重。初始意味着惩罚只适用于轨迹的前几个姿势。
visualize(): 查看planner中规划的具体内容。但这个只是一个接口，覆盖此方法，以提供一个接口，一次性执行所有与规划器相关的可视化。
isTrajectoryFeasible(void): 检查规划的路径是否可行。
computeCurrentCost: 计算并返回当前优化图的代价（支持多个轨迹）。

2.2 TebOptimalPlanner

该类采用g2o框架优化了时间弹性带轨迹。

2.2.1 构造函数

TebOptimalPlanner(const TebConfig& cfg, ObstContainer* obstacles = NULL, RobotFootprintModelPtr robot_model = boost::make_shared<PointRobotFootprint>(), TebVisualizationPtr visual = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL);

必选参数介绍：

cfg: 这里的参数其实是整个算法需要认为调整的参数。前面已经有简单介绍过。
obstacles: 该参数类型是ObstContainer。这个类型在Obstacle类里面，把一个复杂的类型用简单的名字去声明：typedef std::vector<ObstaclePtr>，可以看出，这个本质上是一个指针数组，其中，里面的类型ObstaclePtr也是其他类型的别称：typedef boost::shared_ptr<const Obstacle> ObstacleConstPtr。因此，如果需要传入这个类型的参数，只需要根据类型创建相应的变量，并传入指针就好。

可选参数介绍：

robot_model: 这里设置机器人的足迹形状，用来判断算法生成的路径是否可行。默认为点足迹形状，把机器人看成一个点。
visual: 这里传入机器人可视化的类，如果想要显示路径可视化，可传入对应的参数。如果没有，则会默认生成一个对应的变量。
via_points: 这里传入经过的点的容器类，用来存储自己设定要经过的点。具体可看我之前的分析。

初始化函数，用于构造函数：

void initialize(const TebConfig& cfg, ObstContainer* obstacles = NULL, RobotFootprintModelPtr robot_model = boost::make_shared<PointRobotFootprint>(), TebVisualizationPtr visual = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL)

这里作用为初始化算法，其中的参数的含义和上面构造函数类似。

2.2.2 基类接口中定义的函数

路径规划：

参数为通过一系列包含时间序列的最初的执行计划，一开始的线速度和角速度，以及最后是否允许到达目标的时候还有速度。
```
bool plan(const std::vector<PoseStamped>& initial_plan, const Twist* start_vel = NULL, bool free_goal_vel=false)
```

参数为一开始的位姿，目的地位姿，开始时的线速度和角速度，以及最后是否允许到达目标的时候还有速度。不过这里有两种表示位姿的方式。

bool plan(const Pose& start, const Pose& goal, const Twist* start_vel = NULL, bool free_goal_vel=false)

// 通过另一种方式表示位姿：位置坐标一样，姿态不用四元数表示，使用theta表示
bool plan(const PoseSE2& start, const PoseSE2& goal, const Twist* start_vel = NULL, bool free_goal_vel=false)

获取下达的速度指令。其中，线速度被分解为x和y两个方向的速度，单位为m/s。角速度的单位rad/s。但需要输入参数表明要计算最后一个位姿的序号，用来计算某个位姿的下一刻的速度。
```
bool getVelocityCommand(float& vx, float& vy, float& omega, int look_ahead_poses) const;
```

重置TEB规划器。清除内部的数据结构图和路径。

virtual void clearPlanner()
{ 
     
    clearGraph();
    teb_.clearTimedElasticBand();
}

设定倾向的最初的转弯角度。通过惩罚另外一个完成。如果计划的轨迹在两个成本相近的解决方案（在同一等价类中！）之间摇摆，可以指定一个期望的（初始）转弯方向。检查参数，以便调整惩罚的权重。初始意味着惩罚只适用于轨迹的前几个姿势。
```
virtual void setPreferredTurningDir(RotType dir) { 
     prefer_rotdir_=dir;}
```
发布局部路径规划和位姿序列，比如，需要订阅rviz，这里的话，需要我们针对修改，应该不能使用它原生的程序。
```
virtual void visualize();
```

计算并返回当前优化图的代价（支持多个轨迹）。

virtual void computeCurrentCost(std::vector<double>& cost, double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false)
{ 
     
    computeCurrentCost(obst_cost_scale, viapoint_cost_scale, alternative_time_cost);
    cost.push_back( getCurrentCost() );
}

2.2.3 设定起始速度

优化一个先前已经初始化过的轨迹。不过，这主要是给plan()函数用，我们只需要调用plan函数就可。

bool optimizeTEB(int iterations_innerloop, int iterations_outerloop, bool compute_cost_afterwards = false, double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false);

设置轨迹的初始位姿的速度，通过线速度和角速度去设置。通过plan函数传递相关参数进行调用。
```
void setVelocityStart(const Twist& vel_start);
```
设定期望的目标位姿的速度。通过plan函数调用，是否调用取决于，我们在plan函数中的参数free_goal_vel是否设置为false，并且有指定的速度要求。
```
void setVelocityGoal(const Twist& vel_goal);
```
设定期望的目标位姿的速度为限定速度的最大值。通过plan函数调用，是否调用取决于，我们在plan函数中的参数free_goal_vel是否设置为true。
```
void setVelocityGoalFree() { 
     vel_goal_.first = false;}
```

2.2.4 障碍物操作

重新设置新的障碍物集合。参数指针可为null。注意，这个方法会覆盖在构造函数中初始化的障碍物集合。
```
void setObstVector(ObstContainer* obst_vector) { 
     obstacles_ = obst_vector;}
```

获取障碍物集合。

const ObstContainer& getObstVector() const { 
     return *obstacles_;}

2.2.5 通过点操作

设置新的通过点集合。参数指针可为null。注意，这个方法会覆盖在构造函数中初始化的通过点集合。
```
void setViaPoints(const ViaPointContainer* via_points) { 
     via_points_ = via_points;}
```

获取通过点集合。

const ViaPointContainer& getViaPoints() const { 
     return *via_points_;}

2.2.6 可视化（需要修改）

注册一个TebVisualization类，以启用可视化程序（例如，发布局部路径规划和位姿序列）。
```
void setVisualization(TebVisualizationPtr visualization);
```
发布局部路径规划和位姿序列，比如，需要订阅rviz，这里的话，需要我们针对修改，应该不能使用它原生的程序。
```
virtual void visualize();
```

2.2.7 工具函数

重置TEB规划器。清除内部的数据结构图和路径。

virtual void clearPlanner()
{ 
     
    clearGraph();
    teb_.clearTimedElasticBand();
}

设定倾向的最初的转弯角度。通过惩罚另外一个完成。如果计划的轨迹在两个成本相近的解决方案（在同一等价类中！）之间摇摆，可以指定一个期望的（初始）转弯方向。检查参数，以便调整惩罚的权重。初始意味着惩罚只适用于轨迹的前几个姿势。
```
virtual void setPreferredTurningDir(RotType dir) { 
     prefer_rotdir_=dir;}
```
将为TEB定义的顶点和边注册到g2o::Factory。
```
static void registerG2OTypes();
```

访问内部的TimedElasticBand轨迹。

TimedElasticBand& teb() { 
     return teb_;};
const TimedElasticBand& teb() const { 
     return teb_;}; //只读模式

访问内部的g2o优化器。

boost::shared_ptr<g2o::SparseOptimizer> optimizer() { 
     return optimizer_;};
boost::shared_ptr<const g2o::SparseOptimizer> optimizer() const { 
     return optimizer_;}; // 只读模式

检测最后一次优化是否成功

bool isOptimized() const { 
     return optimized_;};

计算一个给定的优化问题的cost vector（hyper-graph(超图) 必须有）。使用这种方法可以获得关于当前边缘误差/成本（局部成本函数）的信息。成本值的向量是根据不同的边的类型（时间_最佳，障碍物，…）组成的。详细的构成请参考方法声明。最小化时间差的边（EdgeTimeOptimal）的成本对应于所有单次时间差的平方之和。$ \sum_i \Delta T_i^2 $。有时，用户可能希望得到一个与实际轨迹转换时间成比例或相同的值 $\sum_i Δ T_i $。将alternative_time_cost设置为true，以便得到用后一个方程计算的成本，但要检查实现的定义，如果该值被缩放以匹配其他成本值的大小。
```
void computeCurrentCost(double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false);
```