Ompl初探-编程知识

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 Circlecir1 = 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 Circledata=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()