原博链接:https://blog.csdn.net/ljq31446/article/category/7534293
参考学习1
code
#include <ompl/base/SpaceInformation.h>
#include <ompl/base/spaces/SE3StateSpace.h>
#include <ompl/geometric/SimpleSetup.h>
#include <ompl/geometric/planners/rrt/RRT.h>
#include <ompl/config.h>
#include <iostream>
#include <fstream>
#include <ostream>
#include <boost/bind.hpp> //绑定函数
using namespace std;
namespace ob = ompl::base;
namespace og = ompl::geometric;
bool isStateValid(const ob::State *state) {
//抽象类型转换为我们期望类型
const ob::SE3StateSpace::StateType *se3state = state->as<ob::SE3StateSpace::StateType>();
//提取第1、2状态的组成,并转换为我们期望的
const ob::RealVectorStateSpace::StateType *pos = se3state->as<ob::RealVectorStateSpace::StateType>(0);
const ob::SO3StateSpace::StateType *rot = se3state->as<ob::SO3StateSpace::StateType>(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<ob::SE3StateSpace>()->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;
}
结果
学习到的点
-
如何使用不同的规划方法
声明一个构造所需的规划空间: 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) {…} -
使用bind进行绑定函数,输入为空_1占位
-
生成的结果每一行有七位数,前3位表示真实位置,后四位表示so3群的值,参考 https://blog.csdn.net/qq_28448117/article/details/79644920
-
matlab 使用plot3画三维图
参考学习2
code后续放链接
结果
蓝色为直接找到的路径,红色为简化后的路径
学习到的点
- 声明状态路径空间对后续的插值等存在一定的影响
- ss.getSolutionPlannerName();获取路径的规划方法名字,需在sloved之后且存在路径。(默认KPIECE1??可能是某种方式找到最优)
- matlab填充一块区域用fill(x,y);
参考学习3
结果
- 这段代码在找不到路径时也出现found solution
- 时而出现过报错,设定slove时间过大时
- 理论上当无结果时应当找不到,而不是报错
学习的新点
- 对于规划器的使用还需进一步拓展。。部分结构不清楚
参考学习4
结果
- 有时程序报错
学习到的新点
- 对于规划的使用,其主是通过isStateValid来确定可行驶的空间
- 对于dubins等使用,代码与其他不同的地方主要在isStateValid