OMPL官方教程学习Using existing samplers and creating new ones

Available State Samplers

1. 存在两类不同的采样器方案：
2. StateSampler（暂不考虑状态的有效性判断？不管障碍物的存在与否都会进行采样？）
3. ValidStateSampler（考虑到状态的有效性评估，并对非障碍物区域进行分析么？）
4. 这两种采样方案均具备以下三种取点策略：均匀分布、局部密集采样以及高斯分布策略。
5. ValidStateSampler采用了与基础采样器相同的算法框架。
6. 该方案通过模拟基础采样器的方法，在迭代过程中筛选出有效点直至遇到无效状态或达到终止条件。
7. 有效性的验证标准是基于SpaceInformation::isValid方法所定义的判断准则。

These two distinct types of samplers, though sharing similarities in sound, perform unique functions: the ompl::base::StateSampler and the ompl::base::ValidStateSampler. For every kind of state space, it's necessary to define a corresponding derived ompl::base::StateSampler class that facilitates uniform sampling within that specific state space, enables sampling near another existing point within the same space, and allows for Gaussian distribution-based sampling. The ompl::base::ValidStateSampler leverages these underlying primitives by repeatedly generating samples using their respective algorithms until a valid configuration is identified or when reaching the maximum iteration limit set by the system. The validation process for any given configuration is executed through the ompl::base::SpaceInformation::isValid method.

已经定义好的几个ValidStateSampler ：

ompl::base::UniformValidStateSampler ： 均匀采样吧

ompl::base::ObstacleBasedValidStateSampler ：有点类似二分法

该采样器旨在首先寻找一个无效样本和一个有效样本。然后，它从有效状态逐步插值到无效状态。它返回在接近无效状态前最后一个有效的状态。其核心理念在于靠近障碍物的样本能够提高在狭窄通道中找到样本的概率。在解决运动规划问题时，找到这样的样本通常是关键难点。

ompl::base::GaussianValidStateSampler : 按均匀分布随机生成初始状态s0；随后围绕该初始状态s0执行一次采样操作；若首次采样获得的有效状态s1而第二次采样的结果s2无效，则将s1确定为最终的采样结果；否则将执行重置并重新启动采样过程。

ompl::base::MaximizeClearanceValidStateSampler ：采样基于最大 Clearance 的过程？

This sampler resembles ompl::base::UniformValidStateSampler in its functionality. Once a valid state is identified, it endeavors to locate additional valid states that exhibit greater clearance. The sampled state reported is the one with the highest clearance.

1. Using an Existing Sampler

为了实现相同的功能, 我们需要定义一个ValidStateSamplerAllocator. 该类的功能如下: 输入为SpaceInformation, 输出为**ValidStateSamplerPtr.

It is impossible to directly specify the type of sampler within either the SimpleSetup or SpaceInformation classes since each thread requires an independent copy. Instead, we must define and utilize an allocator function (referred to as ompl::base::ValidStateSamplerAllocator) that takes an instance of ompl::base::SpaceInformation and returns a pointer to an allocated ompl::base::ValidStateSampler object. This allocator allows for configuring the valid state sampling logic based on the particular characteristics of the space information provided.

官方给的代码示例从中可以看出：

1. 为了实现目标, 我们计划创建一个函数, 其输入参数类型为SpaceInformation, 输出参数类型为ValidStateSamplerPtr.
2. 函数的名字并不重要.
3. ValidStateSamplerPtr代表基于Valid采样的父类集合, 在实现过程中普遍采用该指针以保持与原有状态空间的一致性.

    namespace ob = ompl::base;
    namespace og = ompl::geometric;
    // 返回的是 ValidStateSamplerPtr对象，输入的是SpaceInformation对象
    ob::ValidStateSamplerPtr allocOBValidStateSampler(const ob::SpaceInformation *si)
    {
    // we can perform any additional setup / configuration of a sampler here,
    // but there is nothing to tweak in case of the ObstacleBasedValidStateSampler.
    // 创建一个ObstacleBasedValidStateSampler采样对象
    return std::make_shared<ob::ObstacleBasedValidStateSampler>(si);
    }

2.Creating a New Valid State Sampler

自己构建一个有效状态采样器，这应用范围就太大了。

官方提供的示例，从中可以看出：

该有效状态采样器必须继承自ValidStateSampler。
在构造过程中需以SpaceInformation为基础以创建实例。
该类必须实现.sample(ob::State *state)函数。
该过程包括以下步骤：
首先将当前的状态对象转换为所需的类型；
然后生成相应的状态实例；
最后调用.isValid(state)方法，
其中si_即为传递给类的对象。

    namespace ob = ompl::base;
    namespace og = ompl::geometric;
     
     
     
    // This is a problem-specific sampler that automatically generates valid
    // states; it doesn't need to call SpaceInformation::isValid. This is an
    // example of constrained sampling. If you can explicitly describe the set valid
    // states and can draw samples from it, then this is typically much more
    // efficient than generating random samples from the entire state space and
    // checking for validity.
    
    // 自定义的有效状态采样器要继承自ValidStateSampler
    class MyValidStateSampler : public ob::ValidStateSampler
    {
    public:
    // 构造函数输入依然是SpaceInformation，用来构造父类
    MyValidStateSampler(const ob::SpaceInformation *si) : ValidStateSampler(si)
    {
        name_ = "my sampler";
    }
    // Generate a sample in the valid part of the R^3 state space
    // Valid states satisfy the following constraints:
    // -1<= x,y,z <=1
    // if .25 <= z <= .5, then |x|>.8 and |y|>.8
    // 需要自己定义sampl函数，格式应该是固定的，输入状态输出是否有效
    bool sample(ob::State *state) override
    {
        // 将输入的状态装换成需要的类型，并获取其数据，也可能没有数据呢
        double* val = static_cast<ob::RealVectorStateSpace::StateType*>(state)->values;
        double z = rng_.uniformReal(-1,1);
        // 根据上面确定的分段空间来确定采样值
        if (z>.25 && z<.5)
        {
            double x = rng_.uniformReal(0,1.8), y = rng_.uniformReal(0,.2);
            switch(rng_.uniformInt(0,3))
            {
                case 0: val[0]=x-1;  val[1]=y-1;  break;
                case 1: val[0]=x-.8; val[1]=y+.8; break;
                case 2: val[0]=y-1;  val[1]=x-1;  break;
                case 3: val[0]=y+.8; val[1]=x-.8; break;
            }
        }
        else
        {
            val[0] = rng_.uniformReal(-1,1);
            val[1] = rng_.uniformReal(-1,1);
        }
        val[2] = z;
        // 判断采样值是否有效，这个isValid函数也要自己定义的。。。。
        assert(si_->isValid(state));
        return true;
    }
    // We don't need this in the example below.
    bool sampleNear(ob::State* /*state*/, const ob::State* /*near*/, const double /*distance*/) override
    {
        throw ompl::Exception("MyValidStateSampler::sampleNear", "not implemented");
        return false;
    }
    protected:
    ompl::RNG rng_;
    };

总结来看：

1. 为了采用采样器这一工具，在设计系统时需要达成与该模块功能特性一致的目标。
2. 为了开发具有独特功能的采样器类，在继承父类的同时需要具体化其核心算法，并实现这两个抽象方法。