本文以基于OMPL库的RRT*算法的实现为例,讲解OMPL库的基本用法。
首先需要通过ob里的RealVectorStateSpace(3)构造出一个三维的状态空间:
// Construct the robot state space in which we're planning 构造状态空间StateSpace
ob::StateSpacePtr space(new ob::RealVectorStateSpace(3));
这样就生成了一个指向一个三维状态空间的指针StateSpacePtr。
然后,设置状态空间的边界,将边界的设置赋予状态空间:
// Set the bounds of space to be in [0,1] 边界条件设置
ob::RealVectorBounds bounds(3);
bounds.setLow(0, - _x_size * 0.5);
bounds.setLow(1, - _y_size * 0.5);
bounds.setLow(2, 0.0);
bounds.setHigh(0, + _x_size * 0.5);
bounds.setHigh(1, + _y_size * 0.5);
bounds.setHigh(2, _z_size);
// 将设置的边界赋予状态空间space,其中as的作用是类型转换
space->as<ob::RealVectorStateSpace>()->setBounds(bounds);
这样就完成了状态空间的构造。
状态信息里存有如何检查一个状态在状态空间中是否有效的函数StateValidityChecker,这里的StateValidityChecker需要根据情况自己去实现:
// Construct a space information instance for this state space 构造状态空间信息SpaceInformation
ob::SpaceInformationPtr si(new ob::SpaceInformation(space));
// Set the object used to check which states in the space are valid 为状态空间设置状态检查函数StateValidChecker
// ValidityChecker肯定要知道状态的信息,也就是si
si->setStateValidityChecker(ob::StateValidityCheckerPtr(new ValidityChecker(si)));
si->setup();
ValidityChecker是继承于StateValidityChecker的,这里需要自己实现状态的判断:
// Our collision checker. For this demo, our robot's state space
class ValidityChecker : public ob::StateValidityChecker
{
public:
ValidityChecker(const ob::SpaceInformationPtr& si) :
ob::StateValidityChecker(si) {}
// Returns whether the given state's position overlaps the
// circular obstacle
bool isValid(const ob::State* state) const
{
// We know we're working with a RealVectorStateSpace in this
// example, so we downcast state into the specific type.
const ob::RealVectorStateSpace::StateType* state3D =
state->as<ob::RealVectorStateSpace::StateType>();
/**
*
*
Extract the robot's (x,y,z) position from its state
*
*
*/
double x,y,z;
x = (*state3D)[0];
y = (*state3D)[1];
z = (*state3D)[2];
return _RRTstar_preparatory->isObsFree(x, y, z);
}
};
问题实例就是针对具体的规划问题,需要告诉规划器的一些信息(比如这里的起点和终点还要优化的目标函数等):
// Set our robot's starting state 定义起点状态
ob::ScopedState<> start(space);
/**
*
*
Finish the initialization of start state
*
*
*/
start[0] = start_pt(0);
start[1] = start_pt(1);
start[2] = start_pt(2);
// Set our robot's goal state 定义终点状态
ob::ScopedState<> goal(space);
/**
*
*
Finish the initialization of goal state
*
*
*/
goal[0] = target_pt(0);
goal[1] = target_pt(1);
goal[2] = target_pt(2);
// Create a problem instance 构造问题实例
/**
*
*
Create a problem instance,
please define variable as pdef
*
*
*/
auto pdef(std::make_shared<ob::ProblemDefinition>(si));
// Set the start and goal states 为问题实例设置起点和终点
pdef->setStartAndGoalStates(start, goal);
// Set the optimization objective
/**
*
*
Set the optimization objective, the options you can choose are defined earlier:
getPathLengthObjective() and getThresholdPathLengthObj()
*
*
*/
// 为问题实例设置优化的目标函数
pdef->setOptimizationObjective(getPathLengthObjective(si));
将状态空间和问题实例定义好之后,就可以交给规划器去执行,规划器在知道状态空间的信息和问题实例之后,即可在状态空间中规划出满足问题实例的规划结果:
//构造规划器Planner
ob::PlannerPtr optimizingPlanner(new og::RRTstar(si));//RRTstar是继承与base::planner的
// Set the problem instance for our planner to solve 将问题实例交给规划器去执行
optimizingPlanner->setProblemDefinition(pdef);
optimizingPlanner->setup();
// attempt to solve the planning problem within one second of
// planning time
ob::PlannerStatus solved = optimizingPlanner->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
og::PathGeometric* path = pdef->getSolutionPath()->as<og::PathGeometric>();
vector<Vector3d> path_points;
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>();
/**
*
*
Trandform the found path from path to path_points for rviz display
*
*
*/
Vector3d path_point;
path_point(0) = (*state)[0];
path_point(1) = (*state)[1];
path_point(2) = (*state)[2];
path_points.push_back(path_point);
}
visRRTstarPath(path_points);
}
void pathFinding(const Vector3d start_pt, const Vector3d target_pt)
{
// Construct the robot state space in which we're planning 构造状态空间StateSpace
ob::StateSpacePtr space(new ob::RealVectorStateSpace(3));
// Set the bounds of space to be in [0,1] 边界条件设置
ob::RealVectorBounds bounds(3);
bounds.setLow(0, - _x_size * 0.5);
bounds.setLow(1, - _y_size * 0.5);
bounds.setLow(2, 0.0);
bounds.setHigh(0, + _x_size * 0.5);
bounds.setHigh(1, + _y_size * 0.5);
bounds.setHigh(2, _z_size);
// 将设置的边界赋予状态空间space,其中as的作用是类型转换
space->as<ob::RealVectorStateSpace>()->setBounds(bounds);
// Construct a space information instance for this state space 构造状态空间信息SpaceInformation
ob::SpaceInformationPtr si(new ob::SpaceInformation(space));
// Set the object used to check which states in the space are valid 为状态空间设置状态检查函数StateValidChecker
// ValidityChecker肯定要知道状态的信息,也就是si
si->setStateValidityChecker(ob::StateValidityCheckerPtr(new ValidityChecker(si)));
si->setup();
// Set our robot's starting state 定义起点状态
ob::ScopedState<> start(space);
/**
*
*
STEP 2: Finish the initialization of start state
*
*
*/
start[0] = start_pt(0);
start[1] = start_pt(1);
start[2] = start_pt(2);
// Set our robot's goal state 定义终点状态
ob::ScopedState<> goal(space);
/**
*
*
STEP 3: Finish the initialization of goal state
*
*
*/
goal[0] = target_pt(0);
goal[1] = target_pt(1);
goal[2] = target_pt(2);
// Create a problem instance 构造问题实例
/**
*
*
STEP 4: Create a problem instance,
please define variable as pdef
*
*
*/
auto pdef(std::make_shared<ob::ProblemDefinition>(si));
// Set the start and goal states 为问题实例设置起点和终点
pdef->setStartAndGoalStates(start, goal);
// Set the optimization objective
/**
*
*
STEP 5: Set the optimization objective, the options you can choose are defined earlier:
getPathLengthObjective() and getThresholdPathLengthObj()
*
*
*/
// 为问题实例设置优化的目标函数
pdef->setOptimizationObjective(getPathLengthObjective(si));
// Construct our optimizing planner using the RRTstar algorithm.
/**
*
*
STEP 6: Construct our optimizing planner using the RRTstar algorithm,
please define varible as optimizingPlanner
*
*
*/
//构造规划器Planner
ob::PlannerPtr optimizingPlanner(new og::RRTstar(si));//RRTstar是继承与base::planner的
// Set the problem instance for our planner to solve 将问题实例交给规划器去执行
optimizingPlanner->setProblemDefinition(pdef);
optimizingPlanner->setup();
// attempt to solve the planning problem within one second of
// planning time
ob::PlannerStatus solved = optimizingPlanner->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
og::PathGeometric* path = pdef->getSolutionPath()->as<og::PathGeometric>();
vector<Vector3d> path_points;
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>();
/**
*
*
STEP 7: Trandform the found path from path to path_points for rviz display
*
*
*/
Vector3d path_point;
path_point(0) = (*state)[0];
path_point(1) = (*state)[1];
path_point(2) = (*state)[2];
path_points.push_back(path_point);
}
visRRTstarPath(path_points);
}
}