Ompl初探

在/ompl-1.x.0/build/Release/bin下有很多生成的demo可执行文件

在终端执行 ./demo_Point2DPlanning
在这里插入图片描述
测试程序

#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;
}

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 src/ompl_test/rrtstar.cc)
target_link_libraries(test_rrtstar ${Boost_LIBRARIES} ${OMPL_LIBRARIES})

可视化脚本

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()

python3 mpplot.py

在这里插入图片描述

需要安装mpl_toolkits

python中绘制3D图形，依旧使用常用的绘图模块matplotlib，但需要安装mpl_toolkits工具包，安装方法如下：windows命令行进入到python安装目录下的Scripts文件夹下，执行： pip install --upgrade matplotlib即可、

不过这里暂时没用

import numpy
import matplotlib.pyplot as plt
from matplotlib.patches import Circle

data=numpy.loadtxt("path_rrt_star.dat")
T=numpy.arctan2(data[:,0],data[:,1])

plt.scatter(data[:,0],data[:,1],s=80,c=T,alpha=0.5)
plt.show()