OMPL官方教程学习Optimal Planning Tutorial

Optimal Planning Tutorial

Optimal Planning应该是最优规划吧，规划算法在某种程度上是最优，比如RRT*等等。

官方教程中说使用要进行Optimal Planning 与一般的规划方法使用流程基本一样，只不过多了一下两点：

1. 需要为ompl::base::ProblemDefinition 对象设置目标函数optimization objective
2. 需要使用最有规划器optimizing planner，而不是一般的规划器。

尴尬我都不知道一般的规划器使用流程。。。。。官方在第一篇教程中有简单介绍，这里写一下：

1. 一般的规划的步骤：
   1. 使用SimpleSetup 对象则：
      1. 应该使用setPlanner() 完成
   2. 不使用SimpleSetup对象则：
      1. 创建SpaceInformation
      2. 创建ProblemDefinition 根据SpaceInformation
      3. 为ProblemDefinition 设置起点终点
      4. 创建planner，根据SpaceInformation 以及所使用的规划方法
      5. 为planner 设置ProblemDefinition
      6. planner->setup()，启动？
      7. planner->slove()，求解.
2. 最优规划的步骤：
   1. 教程中只给了不使用SimpleSetup 的情况，下面列出不同之处：
      1. 创建SpaceInformation
      2. 创建ProblemDefinition 根据SpaceInformation
      3. 为ProblemDefinition 设置起点终点
      4. 创建OptimizationObjective
      5. 为ProblemDefinition 设置OptimizationObjective
      6. 创建planner，根据SpaceInformation 以及所使用的最优规划方法
      7. 为planner 设置ProblemDefinition
      8. planner->setup()，启动？
      9. planner->slove()，求解.

1.Finding the shortest path

官方提供的场景：从(0,0)到(1,1)的一个二维区域，障碍物在(0.5,0.5)半径为0.25

We’ll demonstrate OMPL’s optimal planning framework with an example. In this example, our robot is represented as a (x,y) coordinate on a square, where (0,0) is the square’s bottom-left corner and (1,1) is the square’s top-right corner. There is also an obstacle in this square that the robot cannot pass through; this obstacle is a circle of radius 0.25 centered at (0.5,0.5). To reflect this environment, we use a two-dimensional ompl::base::RealVectorStateSpace and define our state validity checker as follows:

官方自定义了一个state validity checker，我们StateValidityChecker与 StateSpace 对象是必须实现的两个对象。实现StateValidityChecker 主要就是为了实现两个虚函数，不过这两个虚函数在基类也有实现，这里重新实现了：

1. isValid：判断状态是否有效
2. clearance：当前状态距离最近的无效状态之间的距离

   Report the distance to the nearest invalid state when starting from state. If the distance is negative, the value of clearance is the penetration depth.

// Our collision checker. For this demo, our robot's state space
// lies in [0,1]x[0,1], with a circular obstacle of radius 0.25
// centered at (0.5,0.5). Any states lying in this circular region are
// considered "in collision".
class ValidityChecker : public ob::StateValidityChecker
{
   
   
public:
    ValidityChecker(const ob::SpaceInformationPtr& si) :
        ob::StateValidityChecker(si) {
   
   }
 
    // Returns whether the given state's position overlaps the
    // circular obstacle
    bool isValid(const ob::State* state) const
    {