OMPL 实现 2D RRTstar

RRT 实现 2D RRT star 的例子，注释后续添加。

 参考：http://ompl.kavrakilab.org/optimalPlanningTutorial.html

 主要3个文件： rrtstar.cpp    CMakeLists.txt    plot_rrtstar.py

其中rrtstar.cpp如下：

#include <ompl/base/SpaceInformation.h>
#include <ompl/base/spaces/SE3StateSpace.h>
#include <ompl/base/StateSpace.h>
#include <ompl/base/Path.h>
#include <ompl/base/spaces/RealVectorBounds.h>
#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/base/StateValidityChecker.h>
#include <ompl/base/OptimizationObjective.h>
#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
// #include <ompl/geometric/planners/rrt/RRT.h>
#include <ompl/geometric/planners/rrt/RRTstar.h>
#include <ompl/geometric/SimpleSetup.h>
#include <ompl/config.h>

#include <iostream>
#include <fstream>
#include <ostream>
#include "boost/bind.hpp"

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

class ValidityChecker: public ob::StateValidityChecker{
  public:
  ValidityChecker(const ob::SpaceInformationPtr & si):
    ob::StateValidityChecker(si){}
  
  bool isValid(const ob::State* state) const{
    return this->clearance(state)>0.0;
  }
  
  double clearance(const ob::State* state) const{
    const ob::RealVectorStateSpace::StateType* state2D = 
       state->as<ob::RealVectorStateSpace::StateType>();

       double x = state2D->values[0];
       double y = state2D->values[1];

       return sqrt((x - 0.5)*(x - 0.5) + (y-0.5)*(y-0.5)) - 0.25;
  }
};

ob::OptimizationObjectivePtr getPathLengthObjective(const ob::SpaceInformationPtr& si){
  return ob::OptimizationObjectivePtr(new ob::PathLengthOptimizationObjective(si));
}

void OptimalPlanningRRTStar(){
  ob::StateSpacePtr space(new ob::RealVectorStateSpace(2));

  space->as<ob::RealVectorStateSpace>()->setBounds(0.0,1.0);

  ob::SpaceInformationPtr si(new ob::SpaceInformation(space));
  si->setStateValidityChecker(ob::StateValidityCheckerPtr(new ValidityChecker(si)));
  si->setup();

  ob::ScopedState<> start(space);
  start->as<ob::RealVectorStateSpace::StateType>()->values[0] = 0.0;
  start->as<ob::RealVectorStateSpace::StateType>()->values[1] = 0.0;

  ob::ScopedState<> goal(space);
  goal->as<ob::RealVectorStateSpace::StateType>()->values[0] = 1.0;
  goal->as<ob::RealVectorStateSpace::StateType>()->values[1] = 1.0;

  ob::ProblemDefinitionPtr pdef(new ob::ProblemDefinition(si));
  pdef->setStartAndGoalStates(start,goal);
  pdef->setOptimizationObjective(getPathLengthObjective(si));

  ob::PlannerPtr optimizingPlanner(new og::RRTstar(si));
  optimizingPlanner->setProblemDefinition(pdef);
  optimizingPlanner->setup();

  ob::PlannerStatus solved = optimizingPlanner->solve(1.0);

  if(solved){
    std::cout<<"---------solved!--------"<<std::endl;
    og::PathGeometric* path = pdef->getSolutionPath()->as<og::PathGeometric>();
    std::cout<<"path point number="<<path->getStateCount()<<std::endl;
    std::ofstream ofs0("../path_rrt_star.dat");

    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>();
      std::cout<<"i="<<path_idx<<",(x,y)="<<state->values[0]<<","<<state->values[1]<<std::endl;
      ofs0<<state->values[0]<<" "<<state->values[1]<<std::endl;
    }
    pdef->getSolutionPath()->print(std::cout);
  }

}

int main(){
  OptimalPlanningRRTStar();
  return 0;
}

 CMakeLists.txt  如下：

cmake_minimum_required(VERSION 2.8)

add_definitions(
  -Wall
  -g
  -O2
  -std=c++11
)

project(OMPL_learning)

find_package(OMPL REQUIRED)

include_directories(${OMPL_INCLUDE_DIRS})
link_directories(${OMPL_LIBRARY_DIRS})
add_definitions(${OMPL_DEFINITIONS})

find_package(Boost COMPONENTS system REQUIRED)

add_executable(test_rrtstar rrtstar.cpp)
target_link_libraries(test_rrtstar ${Boost_LIBRARIES} ${OMPL_LIBRARIES})

Python 绘图脚本 plot_rrtstar.py

from mpl_toolkits.mplot3d import Axes3D
import numpy
import matplotlib.pyplot as plt
from matplotlib.patches import Circle

cir1 = Circle(xy = (0.5,0.5),radius=0.25,alpha=0.5)
data = numpy.loadtxt('path_rrt_star.dat')
#data1= numpy.loadtxt('obstacle.dat')
fig = plt.figure()
ax=fig.add_subplot(111)
ax.add_patch(cir1)
#ax = fig.gca(projection='3d')
plt.plot(data[:,0],data[:,1],'.-')
plt.hold('on')
plt.grid('on')
#plt.fill(data1[:,0],data1[:,1],'.-')
#plt.hold('on')
#plt.grid('on')
plt.show()

 效果图如下：