ompl学习---参考csdn学习资料

原博链接：https://blog.csdn.net/ljq31446/article/category/7534293

参考学习1

code

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include  //绑定函数

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

bool isStateValid(const ob::State *state) {
	//抽象类型转换为我们期望类型
	const ob::SE3StateSpace::StateType *se3state = state->as();
	//提取第1、2状态的组成，并转换为我们期望的
	const ob::RealVectorStateSpace::StateType *pos = se3state->as(0);
	const ob::SO3StateSpace::StateType *rot = se3state->as(1);

	//确定状态是否可行，这里一直为true，避免编译器警告
	return (const void *)rot != (const void *)pos;
}

void planWithSimpleSetup() {
	//声明我们规划所在的空间维度
	ob::StateSpacePtr space(new ob::SE3StateSpace()); 
	//设置三维空间的边界
	ob::RealVectorBounds bounds(3);
	bounds.setHigh(1);
	bounds.setLow(-1);
	space->as()->setBounds(bounds);
	//定义一个简易类
	og::SimpleSetup ss(space);

	//路径约束检查,使用bind绑定函数，参考 https://blog.csdn.net/giepy/article/details/45046737
	ss.setStateValidityChecker(boost::bind(&isStateValid,_1));
	// 随机创建一个起始点和目标点
	ob::ScopedState<> start(space),goal(space);
	start.random();
	goal.random();
	start.print();
	//加入起终点
	ss.setStartAndGoalStates(start, goal);
	//设定规划方法
	ob::PlannerPtr planner(new og::RRT(ss.getSpaceInformation()));
	ss.setPlanner(planner);
	//在规划的时间内解决
	ob::PlannerStatus solved = ss.solve(1.0);

	//解决则导出生成的路径
	if (solved) {
		cout << "Found solution\n" << endl;
		ofstream osf0("path0.txt");
		ss.getSolutionPath().printAsMatrix(osf0);
		ofstream osf1("path1.txt");
		ss.simplifySolution();
		ss.getSolutionPath().printAsMatrix(osf1);
	}
	else
		cout << "No found" << endl;
}

int main(int, char**) {
	cout << "OMPL_VERSION:" << OMPL_VERSION << endl;
	planWithSimpleSetup();
	return 0;
}

结果

学习到的点

1. 如何使用不同的规划方法
   声明一个构造所需的规划空间： og::SimpleSetup ss(space);
   将起终点加入到： ss.setStartAndGoalStates(start, goal);
   是否对行驶路径进行约束： isStateValid
   添加规划方法（在\ompl\geometric\planners里有各种规划方法 ）： ob::PlannerPtr planner(new og::InformedRRTstar(ss.getSpaceInformation()));
   加入到规划空间中： ss.setPlanner(planner);
   生成规划路径： ss.solve(）
   若找到路径 则进行···处理 ： if (sloved) {…}

2. 使用bind进行绑定函数，输入为空_1占位

3. 生成的结果每一行有七位数，前3位表示真实位置，后四位表示so3群的值，参考 https://blog.csdn.net/qq_28448117/article/details/79644920

4. matlab 使用plot3画三维图

参考学习2

code后续放链接

结果

蓝色为直接找到的路径，红色为简化后的路径

学习到的点

1. 声明状态路径空间对后续的插值等存在一定的影响
2. ss.getSolutionPlannerName();获取路径的规划方法名字，需在sloved之后且存在路径。（默认KPIECE1？？可能是某种方式找到最优）
3. matlab填充一块区域用fill(x,y);

参考学习3

结果

1. 这段代码在找不到路径时也出现found solution
2. 时而出现过报错，设定slove时间过大时
3. 理论上当无结果时应当找不到，而不是报错
   

学习的新点

1. 对于规划器的使用还需进一步拓展。。部分结构不清楚

参考学习4

结果

1. 
2. 
3. 有时程序报错

学习到的新点

1. 对于规划的使用，其主是通过isStateValid来确定可行驶的空间
2. 对于dubins等使用，代码与其他不同的地方主要在isStateValid

上述源码链接：https://download.csdn.net/download/dan__ran/10797807