ompl learning --- reference csdn learning materials

Bo original link: https://blog.csdn.net/ljq31446/article/category/7534293

Reference to learn 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;
}

result

Learning to spot

1. How to use different methods of planning
   a construction plan space required declaration: OG :: SimpleSetup SS (Space);
   will be added to the end of play: ss.setStartAndGoalStates (Start,. Goal);
   whether the travel path constraint: isStateValid
   add Planning (\ geometric \ planners there in \ ompl various planning methods): ob :: PlannerPtr Planner (new new OG :: InformedRRTstar (ss.getSpaceInformation ()));
   added to the planning space: ss.setPlanner (Planner);
   generating path planning: ss.solve ()
   if a path is found for ????? process: IF (sloved) {...}

2. Use bind binding function, input is empty placeholder _1

3. Results generated each row has seven digits, the first three bits represent the true position of the last four values of the group represented so3, reference https://blog.csdn.net/qq_28448117/article/details/79644920

4. Use matlab draw a three-dimensional map plot3

Reference to learn 2

Follow the link code release

result

Blue is the direct path to find the red after a simplified path

Learning to spot

1. Claim Status path space certain influence on the subsequent interpolation, etc.
2. ss.getSolutionPlannerName (); Gets the name of the method path planning is required and there is a path after sloved. (Default KPIECE1 ?? may be some way to find the optimal)
3. matlab a region filled with a fill (x, y);

Reference to learn 3

result

1. This code also appears found solution can not be found in the path
2. Sometimes there have been an error, when the time is too large set slove
3. In theory it should not be found when there is no result, rather than an error
   

The new point of learning

1. For use planner needs to be further expanded. . Part of the structure is not clear

Reference to learn 4

result

1. 
2. 
3. Sometimes a program error

Learn new point

1. For planning, which is the main driving space may be determined by isStateValid
2. For dubins such as the use code and various other places, mainly in isStateValid

Source link above: https://download.csdn.net/download/dan__ran/10797807