ompl之RRT导航

以下转载自：运动规划学习笔记1——探索OMPL - 古月居

求解过程：利用OMPL提供的类和方法来规划，基本过程包括以下几个部分
♥ 确认实际问题的状态空间 （如SE(3)）
♥ 从OMPL提供的类中构建一个状态空间（如ompl::base::SE3StateSpace is appropriate）
♥ 设置状态空间的边界
♥ 定义状态有效性的判断方法
♥ 定义起点和终点的状态
♥ 求解和可视化

//注：这是在ROS里面写的，虽然这一部分跟ROS暂时没关系。
#include <ros/ros.h>
#include <iostream>
 
#include <ompl/base/spaces/SE3StateSpace.h>
 
//使用ompl::geometric::SimpleSetup类
#include <ompl/geometric/SimpleSetup.h>
//不使用ompl::geometric::SimpleSetup类
#include <ompl/base/SpaceInformation.h>
#include <ompl/geometric/planners/rrt/RRTConnect.h>
 
namespace ob = ompl::base;
namespace og = ompl::geometric;
 
//状态检查函数
bool isStateValid(const ob::State *state)
{
	// cast the abstract state type to the type we expect
	// 将抽象状态类型转换为我们期望的类型
	const auto *se3state = state->as<ob::SE3StateSpace::StateType>();
 
	// extract the first component of the state and cast it to what we expect 
	// 提取状态的第一个组件并将其转换为我们所期望的
	const auto *pos = se3state->as<ob::RealVectorStateSpace::StateType>(0);
 
	// extract the second component of the state and cast it to what we expect
	// 提取状态的第二个组件并将其转换为我们所期望的
	const auto *rot = se3state->as<ob::SO3StateSpace::StateType>(1);
 
	// check validity of state defined by pos & rot
 
 
	// return a value that is always true but uses the two variables we define, so we avoid compiler warnings
	// 返回一个始终为true但使用我们定义的两个变量的值
	return (const void*)rot != (const void*)pos;
}
 
//简单规划：使用ompl::geometric::SimpleSetup类
void planWithSimpleSetup()
{
	// 状态空间：构建
	auto space(std::make_shared<ob::SE3StateSpace>());
 
	// 状态空间：边界
	ob::RealVectorBounds bounds(3);
	bounds.setLow(-1);
	bounds.setHigh(1);
	space->setBounds(bounds);
 
	// 配置类：构建
	og::SimpleSetup ss(space);
 
	// 配置类：设置状态有效性检查器
	ss.setStateValidityChecker([](const ob::State *state) { return isStateValid(state); });
 
	// 配置类：设置起终点 
	ob::ScopedState<> start(space);
	start.random();
	ob::ScopedState<> goal(space);
	goal.random();
	ss.setStartAndGoalStates(start, goal);
 
	// this call is optional, but we put it in to get more output information
	ss.setup();
	ss.print();
 
	// attempt to solve the problem within one second of planning time
	ob::PlannerStatus solved = ss.solve(1.0);
 
	if (solved)
	{
		std::cout << "Found solution:" << std::endl;
		// print the path to screen
		ss.simplifySolution();
		ss.getSolutionPath().print(std::cout);
	}
	else
		std::cout << "No solution found" << std::endl;
}
 
//完整规划：不使用ompl::geometric::SimpleSetup类
void plan()
{
	// 状态空间：构建
	auto space(std::make_shared<ob::SE3StateSpace>());
 
	// 状态空间：边界
	ob::RealVectorBounds bounds(3);
	bounds.setLow(-1);
	bounds.setHigh(1);
	space->setBounds(bounds);
 
	// 空间信息：构建
	auto si(std::make_shared<ob::SpaceInformation>(space));
 
	// 空间信息：设置状态有效性检查器
	si->setStateValidityChecker(isStateValid);
 
	// 问题实例：构建
	auto pdef(std::make_shared<ob::ProblemDefinition>(si));
 
	// 问题实例：设置起终点
	ob::ScopedState<> start(space);
	start.random();
	ob::ScopedState<> goal(space);
	goal.random();
	pdef->setStartAndGoalStates(start, goal);
 
	// 规划方法：构建
	auto planner(std::make_shared<og::RRTConnect>(si));
 
	// 规划方法：实际问题
	planner->setProblemDefinition(pdef);
 
	// 规划方法：初始化
	planner->setup();
 
	// 打印空间信息和问题实例
	si->printSettings(std::cout);
	pdef->print(std::cout);
 
	// 求解
	ob::PlannerStatus solved = planner->ob::Planner::solve(1.0);
 
	if (solved)
	{
		// get the goal representation from the problem definition (not the same as the goal state)
		// and inquire about the found path
		ob::PathPtr path = pdef->getSolutionPath();
		std::cout << "Found solution:" << std::endl;
 
		// print the path to screen
		path->print(std::cout);
	}
	else
		std::cout << "No solution found" << std::endl;
}
 
int main(int argc, char** argv)
{
	ros::init(argc, argv, "rigid_body_planning");
	ros::NodeHandle nh;
 
	std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
	plan();
	std::cout << std::endl << std::endl;
	planWithSimpleSetup();	
 
	return 0;
}