概述

本文主要介绍如何使用ompl库实现自己的随机采样规划算法。由于OMPL库已经封装了功能完善的采样算法、最临近节点搜索算法等。基于这些算法接口，可以很容易地实现各种改进型的随机搜索树算法。

限制要求

基于OMPL库实现新的运动规划算法非常简单，只有两个要求：

1. 新的算法的类需公有继承ompl::base::Planner
2. 从继承的ompl::base::Planner类中，实现solve()函数

可选特性

除了上述严格要求之外，为了便于集成，还有一些可集成的函数和应遵循的做法。它们不作要求，但为了简单和一致强烈建议包含下述特性

配置规划器属性

更新ompl::base::Planner的成员变量ompl::base::PlannerSpecs，该结构体表示规划器是否包含相关特性。如果在新的规划器中更新这些属性，对于用户来讲，可以更好的使用该规划器。

重载setup()函数

为了执行任何一次性的方法，可以重载函数setup()。

注意:setup()方法要确保只能调用一次，且不能每次使用solve()前调用它。

更新问题定义

当在solve()函数中找到一个可行路径，使用ompl::base::ProblemDefinition中的addSolutionPath成员函数，将有效路径添加到相应的实例化对象中。其中ompl::base::ProblemDefinition也是ompl::base::Planner的子类。

返回规划状态

在执行完solve()函数后，需通过ompl::base::PlannerStatus返回丰富的状态值。

配置停止条件

应该将ompl::base::PlannerTerminationCondition参数递给solve()函数，且当给定条件评定为真时，solve()函数应该尽可能快的返回。

反复规划

重复调用solve()函数，不应从头开始新的规划过程，而应从先前中断的地方继续进行搜索。

clear()函数

提供ompl::base::Planner::clear()函数的实现，这个函数应该释放所有由规划器申请的内存空间并将规划器恢复到可以再次调用ompl::base::Planner::solve()函数的状态，这样就无需将先前调用的信息传递给solve()函数。如果有必要的话，clear()函数可以设置ompl::base::Planner::setup_参数为false，以通知solve函数，setup()函数需要再次调用。

规划数据格式

提供一个ompl::bas::Planner::getPlannerData()的实例，其可以将内部的数据结构转化为ompl::base::PlannerData的图形化实现。因为它允许用户检查数据结构，所以该数据格式对调试特别有用。

新规划器的模板

#include <ompl/base/Planner.h>
 
// often useful headers:
#include <ompl/util/RandomNumbers.h>
#include <ompl/tools/config/SelfConfig.h>
 
namespace ompl
{
    
    class myNewPlanner : public base::Planner
    {
    
    public:
 
        myNewPlanner(const base::SpaceInformationPtr &si) : base::Planner(si, "the planner's name")
        {
    
            // 规划器的规格 (ompl::base::PlannerSpecs)
            specs_.approximateSolutions = ...;
            specs_.recognizedGoal = ...;
            ...
        }
 
        virtual ~myNewPlanner(void)
        {
    
            // 释放所有分配的内存空间
        }
 
        virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
        {
    
            // 确保规划器配置正确;ompl::base::Planner::checkValidity确保至少一个输入状态和一个ompl::base::Goal对象
            checkValidity();
 
            // 从ompl::base::ProblemDefinition实例化成员(pdef_)中，获取一个目标句柄
            base::Goal *goal = pdef_->getGoal().get();
 
            // 通过PlannerInputStates(pis_)协助，获取输入状态
            while (const base::State *st = pis_.nextStart())
            {
    
                // st将包含一个开始状态。通常这个状态将会被克隆并插入到规划器的数据结构中
            }
 
            // 如果需要，采样状态从目标区域获得
            const base::State *st = pis_.nextGoal(ptc);
            // 或者如果可行的话，采样一个新的目标状态
            const base::State *st = pis_.nextGoal();
 
            // 定期检查ptc()函数是否返回真，如果返回真，终止规划
            while (ptc() == false)
            {
    
                // 规划从这儿开始
 
                // 根据需要，调用SpaceInformation的子程序，例如
                // 1、si_->allocStateSampler() 函数用于采样;
                // 2、si_->checkMotion(state1, state2) 函数用于状态检测
 
 				// 使用一个目标点去评估一个采样状态是否满足目标要求
                // use the Goal pointer to evaluate whether a sampled state satisfies the goal requirements
 
                // 使用log宏用于信息传递，例如logInfo("Planner found a solution!");
            }
 
            // When a solution path is computed, save it here
            // 当计算出一个可行路径的解，将它保存这儿
            pdef_->addSolutionPath(...);
 
            // Return a value from the PlannerStatus enumeration.
            // See ompl::base::PlannerStatus for the possible return values
            // 从PlannerStatus枚举中，返回一个值
            // 参照ompl::base::PlannerStatus获取可行的返回值
            return base::PlannerStatus::EXACT_SOLUTION;
        }
 
        virtual void clear(void)
        {
    
            Planner::clear();
            // clear the data structures here
        }
 
        // optional, if additional setup/configuration is needed, the setup() method can be implemented
        virtual void setup(void)
        {
    
            Planner::setup();
 
            // perhaps attempt some auto-configuration
            SelfConfig sc(si_, getName());
            sc.configure...
        }
 
        virtual void getPlannerData(base::PlannerData &data) const
        {
    
            // fill data with the states and edges that were created
            // in the exploration data structure
            // perhaps also fill control::PlannerData
        }
 
    };
}

参考

• OMPL官方说明