OMPL RRTConnet 生成路径和可视化

最新推荐文章于 2024-07-18 16:30:00 发布

weixin_30316097

最新推荐文章于 2024-07-18 16:30:00 发布

阅读量1k

点赞数 2

原文链接：http://www.cnblogs.com/flyinggod/p/10809488.html

版权

本文介绍了使用OMPL库的RRTConnect算法进行路径规划的方法，包括源代码实现和Python环境下的路径可视化。通过官方文档提供的资源，读者可以了解详细步骤并观察原始路径与简化路径的区别。

摘要由CSDN通过智能技术生成

默认规划路径算法和RRTConnet路径规划算法生成路径

1. 源代码

#include <ompl/base/SpaceInformation.h>
#include <ompl/base/spaces/SE3StateSpace.h>
#include <ompl/geometric/planners/rrt/RRTConnect.h>
#include <ompl/geometric/SimpleSetup.h>

#include <ompl/config.h>
#include <iostream>

#include <fstream>
#include <ostream>

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

	// extra