本文以基于OMPL库的RRT*算法的实现为例，讲解OMPL库的基本用法。

1. 构造状态空间

首先需要通过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);

这样就完成了状态空间的构造。

2. 配置状态信息

状态信息里存有如何检查一个状态在状态空间中是否有效的函数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 obstaclebool 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);}
};

3. 设置问题实例

问题实例就是针对具体的规划问题，需要告诉规划器的一些信息（比如这里的起点和终点还要优化的目标函数等）：

// 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));

4. 执行规划器

将状态空间和问题实例定义好之后，就可以交给规划器去执行，规划器在知道状态空间的信息和问题实例之后，即可在状态空间中规划出满足问题实例的规划结果：

//构造规划器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 pathog::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);
}

5. 完整代码

void pathFinding(const Vector3d start_pt, const Vector3d target_pt)
{// Construct the robot state space in which we're planning 构造状态空间StateSpaceob::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 构造状态空间信息SpaceInformationob::SpaceInformationPtr si(new ob::SpaceInformation(space));// Set the object used to check which states in the space are valid 为状态空间设置状态检查函数StateValidChecker// ValidityChecker肯定要知道状态的信息，也就是sisi->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***/ //构造规划器Plannerob::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 timeob::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 pathog::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);       }
}

基于OMPL库的RRT*算法实现相关推荐

python分类算法的应用_Python基于sklearn库的分类算法简单应用示例
Python基于sklearn库的分类算法简单应用示例来源:中文源码网浏览: 次日期:2018年9月2日 [下载文档: Python基于sklearn库的分类算法简单应用示例.tx ...
Math之ARIMA：基于statsmodels库利用ARIMA算法对太阳黑子年数据(来自美国国家海洋和大气管理局)实现回归预测(ADF检验+LB检验+DW检验+ACF/PACF图)案例
Math之ARIMA:基于statsmodels库利用ARIMA算法对太阳黑子年数据(来自美国国家海洋和大气管理局)实现回归预测(ADF检验+LB检验+DW检验+ACF/PACF图)案例目录
Math之ARIMA：基于statsmodels库利用ARIMA算法(ADF检验+差分修正+ACF/PACF图)对上海最高气温实现回归预测案例
Math之ARIMA:基于statsmodels库利用ARIMA算法(ADF检验+差分修正+ACF/PACF图)对上海最高气温实现回归预测案例目录基于statsmodels库利用ARIMA算法对上 ...
基于采样的路径规划算法RRT和代码实现
文章目录前言一.概率路图法 1.1 采样阶段 1.2 搜索阶段 1.3 Lazy collision-checking 二.快速扩展随机树 2.1 RRT算法流程 2.2 RRT 算法改进 2.3 ...
RRT算法(快速拓展随机树)的Python实现
""" <基于智能优化与RRT算法的无人机任务规划方法研究>博士论文 <基于改进人工势场法的路径规划算法研究>硕士论文""&q ...
2021-11-06 ompl运动规划库的规划算法
官网介绍available planners 如何实现新的运动规划算法机翻: 几何规划器此类别中的规划器仅考虑系统的几何和运动学约束.假设任何可行的路径都可以变成动态可行的轨迹.这些规划器中的任何 ...
ML之NB：(NLP)基于sklearn库利用不同语种数据集训练NB(朴素贝叶斯)算法，对新语种进行语种检测
ML之NB:(NLP)基于sklearn库利用不同语种数据集训练NB(朴素贝叶斯)算法,对新语种进行语种检测目录输出结果训练数据集设计思路核心代码输出结果测试01:I love you ...
ML之分类预测：基于sklearn库的七八种机器学习算法利用糖尿病(diabetes)数据集(8→1)实现二分类预测
ML之分类预测:基于sklearn库的七八种机器学习算法利用糖尿病(diabetes)数据集(8→1)实现二分类预测目录输出结果数据集展示输出结果 1.k-NN 2.LoR 4.DT 5.RF ...
清华大学开源迁移学习算法库：基于PyTorch实现已有算法
点上方蓝字计算机视觉联盟获取更多干货在右上方 ··· 设为星标 ★,与你不见不散仅作学术分享,不代表本公众号立场,侵权联系删除转载于:机器之心 AI博士笔记系列推荐周志华<机器学习> ...

基于OMPL库的RRT*算法实现