基于OMPL库的RRT*算法实现

本文以基于OMPL库的RRT*算法的实现为例，讲解OMPL库的基本用法。

1. 构造状态空间

首先需要通过ob里的RealVectorStateSpace(3)构造出一个三维的状态空间：

// Construct the robot state space in which we're planning 构造状态空间StateSpace
ob::StateSpacePtr space(new ob::RealVectorStateSpace(3));

这样就生成了一个指向一个三维状态空间的指针StateSpacePtr。
然后，设置状态空间的边界，将边界的设置赋予状态空间：

// Set the bounds of space to be in [0,1] 边界条件设置
ob::RealVectorBounds bounds(3);
bounds.setLow(0, - _x_size * 0.5);
bounds.setLow(1, - _y_size * 0.5);
bounds.setLow(2, 0.0);
bounds.setHigh(0, + _x_size * 0.5);
bounds.setHigh(1, + _y_size * 0.5);
bounds.setHigh(2, _z_size);
// 将设置的边界赋予状态空间space，其中as的作用是类型转换
space->as<ob::RealVectorStateSpace>()->setBounds(bounds);

这样就完成了状态空间的构造。

2. 配置状态信息

状态信息里存有如何检查一个状态在状态空间中是否有效的函数StateValidityChecker，这里的StateValidityChecker需要根据情况自己去实现：

// Construct a space information instance for this state space 构造状态空间信息SpaceInformation
ob::SpaceInformationPtr si(new ob::SpaceInformation(space));
// Set the object used to check which states in the space are valid 为状态空间设置状态检查函数StateValidChecker
// ValidityChecker肯定要知道状态的信息，也就是si
si->setStateValidityChecker(ob::StateValidityCheckerPtr(new ValidityChecker(si)));
si->setup();

ValidityChecker是继承于StateValidityChecker的，这里需要自己实现状态的判断：

// Our collision checker. For this demo, our robot's state space
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
    {   
        // We know we're working with a RealVectorStateSpace in this
        // example, so we downcast state into the specific type.
        const ob::RealVectorStateSpace::StateType* state3D =
            state->as<ob::RealVectorStateSpace::StateType>();
        /**
        *
        *
        Extract the robot's (x,y,z) position from its state
        *
        *
        */
        double x,y,z;
        x = (*state3D)[0];
        y = (*state3D)[1];
        z = (*state3D)[2];


        return _RRTstar_preparatory->isObsFree(x, y, z);
    }
};

3. 设置问题实例

问题实例就是针对具体的规划问题，需要告诉规划器的一些信息（比如这里的起点和终点还要优化的目标函数等）：

// Set our robot's starting state 定义起点状态
ob::ScopedState<> start(space);
/**
*
*
Finish the initialization of start state
*
*
*/
start[0] = start_pt(0);
start[1] = start_pt(1);
start[2] = start_pt(2);
// Set our robot's goal state 定义终点状态
ob::ScopedState<> goal(space);
/**
*
*
Finish the initialization of goal state
*
*
*/
goal[0] = target_pt(0);
goal[1] = target_pt(1);
goal[2] = target_pt(2);
// Create a problem instance 构造问题实例
/**
*
*
Create a problem instance, 
please define variable as pdef
*
*
*/
auto pdef(std::make_shared<ob::ProblemDefinition>(si));
// Set the start and goal states 为问题实例设置起点和终点
pdef->setStartAndGoalStates(start, goal);
// Set the optimization objective
/**
*
*
Set the optimization objective, the options you can choose are defined earlier:
getPathLengthObjective() and getThresholdPathLengthObj()
*
*
*/  
// 为问题实例设置优化的目标函数
pdef->setOptimizationObjective(getPathLengthObjective(si));

4. 执行规划器

将状态空间和问题实例定义好之后，就可以交给规划器去执行，规划器在知道状态空间的信息和问题实例之后，即可在状态空间中规划出满足问题实例的规划结果：

//构造规划器Planner
ob::PlannerPtr optimizingPlanner(new og::RRTstar(si));//RRTstar是继承与base::planner的
// Set the problem instance for our planner to solve 将问题实例交给规划器去执行
optimizingPlanner->setProblemDefinition(pdef);
optimizingPlanner->setup();

// attempt to solve the planning problem within one second of
// planning time
ob::PlannerStatus solved = optimizingPlanner->solve(1.0);

if (solved)
{
    // get the goal representation from the problem definition (not the same as the goal state)
    // and inquire about the found path
    og::PathGeometric* path = pdef->getSolutionPath()->as<og::PathGeometric>();
        
    vector<Vector3d> path_points;

    for (size_t path_idx = 0; path_idx < path->getStateCount (); path_idx++)
    {
        const ob::RealVectorStateSpace::StateType *state = path->getState(path_idx)->as<ob::RealVectorStateSpace::StateType>(); 
        /**
        *
        *
        Trandform the found path from path to path_points for rviz display
        *
        *
        */ 
        Vector3d path_point;
        path_point(0) = (*state)[0];
        path_point(1) = (*state)[1];
        path_point(2) = (*state)[2];
        path_points.push_back(path_point);
    }
    visRRTstarPath(path_points);       
}

5. 完整代码

void pathFinding(const Vector3d start_pt, const Vector3d target_pt)
{
    // Construct the robot state space in which we're planning 构造状态空间StateSpace
    ob::StateSpacePtr space(new ob::RealVectorStateSpace(3));

    // Set the bounds of space to be in [0,1] 边界条件设置
    ob::RealVectorBounds bounds(3);
    bounds.setLow(0, - _x_size * 0.5);
    bounds.setLow(1, - _y_size * 0.5);
    bounds.setLow(2, 0.0);

    bounds.setHigh(0, + _x_size * 0.5);
    bounds.setHigh(1, + _y_size * 0.5);
    bounds.setHigh(2, _z_size);
    // 将设置的边界赋予状态空间space，其中as的作用是类型转换
    space->as<ob::RealVectorStateSpace>()->setBounds(bounds);
    // Construct a space information instance for this state space 构造状态空间信息SpaceInformation
    ob::SpaceInformationPtr si(new ob::SpaceInformation(space));
    // Set the object used to check which states in the space are valid 为状态空间设置状态检查函数StateValidChecker
    // ValidityChecker肯定要知道状态的信息，也就是si
    si->setStateValidityChecker(ob::StateValidityCheckerPtr(new ValidityChecker(si)));
    si->setup();

    // Set our robot's starting state 定义起点状态
    ob::ScopedState<> start(space);
    /**
    *
    *
    STEP 2: Finish the initialization of start state
    *
    *
    */
    start[0] = start_pt(0);
    start[1] = start_pt(1);
    start[2] = start_pt(2);


    // Set our robot's goal state 定义终点状态
    ob::ScopedState<> goal(space);
    /**
    *
    *
    STEP 3: Finish the initialization of goal state
    *
    *
    */
    goal[0] = target_pt(0);
    goal[1] = target_pt(1);
    goal[2] = target_pt(2);

    // Create a problem instance 构造问题实例

    /**
    *
    *
    STEP 4: Create a problem instance, 
    please define variable as pdef
    *
    *
    */
    auto pdef(std::make_shared<ob::ProblemDefinition>(si));
    // Set the start and goal states 为问题实例设置起点和终点
    pdef->setStartAndGoalStates(start, goal);

    // Set the optimization objective
    /**
    *
    *
    STEP 5: Set the optimization objective, the options you can choose are defined earlier:
    getPathLengthObjective() and getThresholdPathLengthObj()
    *
    *
    */  
    // 为问题实例设置优化的目标函数
    pdef->setOptimizationObjective(getPathLengthObjective(si));

    // Construct our optimizing planner using the RRTstar algorithm.
    /**
    *
    *
    STEP 6: Construct our optimizing planner using the RRTstar algorithm, 
    please define varible as optimizingPlanner
    *
    *
    */ 
    //构造规划器Planner
    ob::PlannerPtr optimizingPlanner(new og::RRTstar(si));//RRTstar是继承与base::planner的
    // Set the problem instance for our planner to solve 将问题实例交给规划器去执行
    optimizingPlanner->setProblemDefinition(pdef);
    optimizingPlanner->setup();

    // attempt to solve the planning problem within one second of
    // planning time
    ob::PlannerStatus solved = optimizingPlanner->solve(1.0);

    if (solved)
    {
        // get the goal representation from the problem definition (not the same as the goal state)
        // and inquire about the found path
        og::PathGeometric* path = pdef->getSolutionPath()->as<og::PathGeometric>();
        
        vector<Vector3d> path_points;

        for (size_t path_idx = 0; path_idx < path->getStateCount (); path_idx++)
        {
            const ob::RealVectorStateSpace::StateType *state = path->getState(path_idx)->as<ob::RealVectorStateSpace::StateType>(); 
            /**
            *
            *
            STEP 7: Trandform the found path from path to path_points for rviz display
            *
            *
            */ 
            Vector3d path_point;
            path_point(0) = (*state)[0];
            path_point(1) = (*state)[1];
            path_point(2) = (*state)[2];
            path_points.push_back(path_point);

        }
        visRRTstarPath(path_points);       
    }
}