OMPL官方教程学习笔记

Geometric planning for a rigid body in 3D

这是官方教程中的第一个示例：讲述了使用OMPL的一般组织架构，结合官方的API关系图更好理解

1. 使用SimplSetup类我们就需要实现：StateValidityChecker与StateSpace
2. 不适用SimpleSetup类我们就需要实现：StateValidityChecker、SpaceInformation、ProblemDefinition、planner、StateSpace
3. 我看了后面教程才知道，使用OMPL的过程中还要按照要求去实现一些函数，因此下面的代码看起来可能有些难以理解，不过这是官方第一个教程，重点是理解上面图中的关系。

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Rice University
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Rice University nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Ioan Sucan */

#include <ompl/base/SpaceInformation.h>
#include <ompl/base/spaces/SE3StateSpace.h>
#include <ompl/geometric/planners/rrt/RRTConnect.h>
#include <ompl/geometric/SimpleSetup.h>

#include <ompl/config.h>
#include <iostream>

namespace ob = ompl::base;
namespace og = ompl::geometric;

bool isStateValid(const ob::State *state)
{
    // cast the abstract state type to the type we expect
    // 这里将抽象基类装换成复合状态空间SE(3),这是看了后面教程才知道的
    const auto *se3state = state->as<ob::SE3StateSpace::StateType>();

    // extract the first component of the state and cast it to what we expect
    // 从复合状态空间SE(3)中提取R^3
    const auto *pos = se3state->as<ob::RealVectorStateSpace::StateType>(0);

    // extract the second component of the state and cast it to what we expect
    // 从复合状态空间SE(3)中提取SO(3)
    const auto *rot = se3state->as<ob::SO3StateSpace::StateType>(1);

    // check validity of state defined by pos & rot


    // return a value that is always true but uses the two variables we define, so we avoid compiler warnings
    return (const void*)rot != (const void*)pos;
}

void plan()
{
    // construct the state space we are planning in
    // 创建规划所在的状态空间
    auto space(std::make_shared<ob::SE3StateSpace>());

    // set the bounds for the R^3 part of SE(3)
    // 创建Rn的状态空间的边界
    ob::RealVectorBounds bounds(3);
    bounds.setLow(-1);
    bounds.setHigh(1);
    // 设置状态空间的边界
    space->setBounds(bounds);

    // construct an instance of  space information from this state space
    // 创建一个SpaceInformation类
    auto si(std::make_shared<ob::SpaceInformation>(space));

    // set state validity checking for this space
    // SpaceInformation对象进行状态点有效性检测，这里直接使用isStateValid，感觉是一样啊
    si->setStateValidityChecker(isStateValid);

    // create a random start state
    ob::ScopedState<> start(space);
    start.random();

    // create a random goal state
    ob::ScopedState<> goal(space);
    goal.random();

    // create a problem instance
    // 构建问题定义对象，需要SpaceInformation对象
    auto pdef(std::make_shared<ob::ProblemDefinition>(si));

    // set the start and goal states
    // 为问题定义对象设置起始点与终点
    pdef->setStartAndGoalStates(start, goal);

    // create a planner for the defined space
    // 创建规划器对象，需要SpaceInformation对象
    auto planner(std::make_shared<og::RRTConnect>(si));

    // set the problem we are trying to solve for the planner
    // 设置规划器解决的问题对象，需要ProblemDefinition
    planner->setProblemDefinition(pdef);

    // perform setup steps for the planner
    // 启动设置？
    planner->setup();

    // print the settings for this space
    si->printSettings(std::cout);

    // print the problem settings
    pdef->print(std::cout);

    // attempt to solve the problem within one second of planning time
    ob::PlannerStatus solved = planner->ob::Planner::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
        // 从问题对象中获取结果
        ob::PathPtr path = pdef->getSolutionPath();
        std::cout << "Found solution:" << std::endl;

        // print the path to screen
        path->print(std::cout);
    }
    else
        std::cout << "No solution found" << std::endl;
}

void planWithSimpleSetup()
{
    // construct the state space we are planning in
    // 创建状态空间对象
    auto space(std::make_shared<ob::SE3StateSpace>());

    // set the bounds for the R^3 part of SE(3)
    // 创建状态空间组件的边界，不理解，每个维度最小值为-1，最大1么
    ob::RealVectorBounds bounds(3);
    bounds.setLow(-1);
    bounds.setHigh(1);
    // 设置状态空间组件的边界
    space->setBounds(bounds);

    // define a simple setup class
    // 创建一个SimpleSetup对象，该对象自动实例化SpaceInformation,ProblemDefinition
    og::SimpleSetup ss(space);

    // set state validity checking for this space
    // 设置状态有效性检查类，本来是要传入一个类，此处传入的是一个函数，StateValidityCheckerFn中有说明
    ss.setStateValidityChecker([](const ob::State *state) { return isStateValid(state); });

    // create a random start state
    // 创建ScopedState(域内状态对象么。。。)
    ob::ScopedState<> start(space);
    // 设置这个状态为随机值
    start.random();

    // create a random goal state
    // 同上
    ob::ScopedState<> goal(space);
    goal.random();
    
    // set the start and goal states
    // 设置起始点与终点
    ss.setStartAndGoalStates(start, goal);

    // this call is optional, but we put it in to get more output information
    ss.setup();
    ss.print();

    // attempt to solve the problem within one second of planning time
    // 设置完毕开始求解
    ob::PlannerStatus solved = ss.solve(1.0);

    if (solved)
    {
        std::cout << "Found solution:" << std::endl;
        // print the path to screen
        // 简化求解的结果？
        ss.simplifySolution();
        // 输出结果
        ss.getSolutionPath().print(std::cout);
    }
    else
        std::cout << "No solution found" << std::endl;
}

int main(int /*argc*/, char ** /*argv*/)
{
    std::cout << "OMPL version: " << OMPL_VERSION << std::endl;

    plan();

    std::cout << std::endl << std::endl;

    planWithSimpleSetup();

    return 0;
}

上述代码流程的简单总结：
一、使用SimpleSetup

1. 创建SE3StateSpace对象
2. 创建RealVectorBounds
3. 设置SE3StateSpace的边界
4. 创建SimpleSetup对象，需要SE3StateSpace
5. 调用SimpleSetup的setStateValidityChecker，状态的有效性检测
6. 创建起始点与终点，ScopedState
7. 调用SimpleSetup的setStartAndGoalStates，设置起点终点
8. 调用SimpleSetup的solve，解算
9. 调用SimpleSetup的simplifySolution，简化结果？
10. 调用SimpleSetup的getSolutionPath，获取路径PathGeometric

二、不使用SimpleSetup

1. 创建SE3StateSpace对象
2. 创建RealVectorBounds，并设置边界
3. 设置SE3StateSpace的边界
4. 创建SpaceInformation对象，需要SE3StateSpace
5. 调用SpaceInformation的setStateValidityChecker，状态的有效性检测
6. 创建起始点与终点，ScopedState
7. 创建ProblemDefinition，需要SpaceInformation
8. 调用ProblemDefinition的setStartAndGoalStates，设置起点终点
9. 创建planner，需要SpaceInformation
10. 调用planner的setProblemDefinition，需要ProblemDefinition
11. 调用planner的solve，求解
12. 调用planner的getSolutionPath，获得路径