软件篇-04-OMPL和FCL用于SLAM轨迹规划
使用OMPL内置的infoRRTstar算法模块和FCL碰撞检测库实现当前点和目标点的轨迹规划,
- 效果图
- 将pointcloud点云转化为OctoMap,用于碰撞检测
// turn the pcl cloud to fcl::CollisionGeometry after octree
// updtae the octomap octomap::OcTree* treeOctomapPtr = new octomap::OcTree( 0.05 );
for(auto p:pclCloud->points) {
if(p.z > groundHeightMax + carTF_zed2.pose.position.z) treeOctomapPtr->updateNode
( octomap::point3d(p.x, p.y, p.z), true );
}
treeOctomapPtr->updateInnerOccupancy();
fcl::OcTree<float>* tree = new fcl::OcTree<float>(std::shared_ptr<const octomap::OcTree>(treeOctomapPtr));
tree_obj = std::shared_ptr<fcl::CollisionGeometry<float>>(tree);
- 设置OMPL轨迹规划限制空间
// set the bounds for the R^3 part of SE(3)
ompl::base::RealVectorBounds bounds(3);
// set X-Y-Z dimensions bound
bounds.setLow(0,bounds_lmin);
bounds.setHigh(0,bounds_lmax);
bounds.setLow(1,bounds_wmin);
bounds.setHigh(1,bounds_wmax);
bounds.setLow(2,bounds_hmin);
bounds.setHigh(2,bounds_hmax);
- 使用FCL检测当前状态是否碰撞
bool Navigation::isStateValid(const ompl::base::State *state)
{
// cast the abstract state type to the type we expect const ompl::base::SE3StateSpace::StateType *se3state = state->as<ompl::base::SE3StateSpace::StateType>();
// extract the first component of the state and cast it to what we expect const ompl::base::RealVectorStateSpace::StateType *pos = se3state->as<ompl::base::RealVectorStateSpace::StateType>(0); // extract the second component of the state and cast it to what we expect const ompl::base::SO3StateSpace::StateType *rot = se3state->as<ompl::base::SO3StateSpace::StateType>(1); fcl::CollisionObject<float> treeObj((tree_obj)); fcl::CollisionObject<float> slamCarObject(slamCar); // check validity of state defined by pos & rot fcl::Vector3f translation(pos->values[0],pos->values[1],pos->values[2]); fcl::Quaternionf rotation(rot->w, rot->x, rot->y, rot->z); slamCarObject.setTransform(rotation, translation); fcl::CollisionRequest<float>requestType(1,false,1,false); fcl::CollisionResult<float> collisionResult; fcl::collide(&slamCarObject,&treeObj, requestType, collisionResult);return(!collisionResult.isCollision());}
在下面的代码将FCL和OMPL相联系,这里的 "std::placeholders::_1"是一个占位符。
si = ompl::base::SpaceInformationPtr(new ompl::base::SpaceInformation(space));
si->setStateValidityChecker(std::bind(&Navigation::isStateValid, this, std::placeholders::_1 ));
- 一切设置好后就可以开始计算路径了
ompl::base::PlannerStatus solved;
try
{
solved = plan->solve(rrtSolutionTimeLimit);
}
catch(ompl::Exception e)
{
ROS_WARN("Error occourred: %s", e.what());
}
ompl::geometric::PathSimplifier* pathBSpline = new ompl::geometric::PathSimplifier(si);
path_smooth = new ompl::geometric::PathGeometric(dynamic_cast<const ompl::geometric::PathGeometric&>(*pdef->getSolutionPath()));
pathBSpline->smoothBSpline(*path_smooth,3);
// ROS_INFO("Smoothed Path:"); // path_smooth->print(std::cout); smooth_msg.header.stamp = ros::Time::now(); smooth_msg.header.frame_id = "map"; for (std::size_t idx = 0; idx < path_smooth->getStateCount (); idx++) { // cast the abstract state type to the type we expect const ompl::base::SE3StateSpace::StateType *se3state = path_smooth->getState(idx)->as<ompl::base::SE3StateSpace::StateType>(); // extract the first component of the state and cast it to what we expect const ompl::base::RealVectorStateSpace::StateType *pos = se3state->as<ompl::base::RealVectorStateSpace::StateType>(0); // extract the second component of the state and cast it to what we expect const ompl::base::SO3StateSpace::StateType *rot = se3state->as<ompl::base::SO3StateSpace::StateType>(1); geometry_msgs::PoseStamped point; point.pose.position.x = pos->values[0]; point.pose.position.y = pos->values[1]; point.pose.position.z = pos->values[2]; point.pose.orientation.x = rot->x; point.pose.orientation.y = rot->y; point.pose.orientation.z = rot->z; point.pose.orientation.w = rot->w; smooth_msg.poses.push_back(point);}
软件篇-04-OMPL和FCL用于SLAM轨迹规划的更多相关文章
- 软件篇-03-基于ORB_SLAM2手写SLAM稠密地图构建实现
本文使用的方法不是从内部修改ORBSLAM2源码以获取稠密点云,而是先从ZED2 sdk获取以摄像头坐标系为描述的三维点云/作为点云地图的一个子集,然后融合IMU与ORB_SLAM2进行实时定位,通过 ...
- 软件篇-05-融合ORB_SLAM2和IMU闭环控制SLAM底盘运动轨迹
前面我们已经得到了当前底盘在世界坐标系中的位姿,这个位姿是通过融合ORB_SLAM2位姿和IMU积分得到的,在当前位姿已知的case下,给SLAM小车设置一个goal,我这里是通过上位机设置,然后 ...
- GSM Sniffing入门之软件篇:GSMTAP抓取与SMS(Short Message Service)
重点介绍如何利用50元左右的设备,抓包并还原SMS短信内容: ps:研究GSM Sniffing纯属个人兴趣,能抓SMS报文只是捡了个明文传输的漏子,切勿用于非法用途.就像sylvain说的,osmo ...
- [知乎]老狼:深入PCI与PCIe之二:软件篇
深入PCI与PCIe之二:软件篇 https://zhuanlan.zhihu.com/p/26244141 我们前一篇文章(深入PCI与PCIe之一:硬件篇 - 知乎专栏)介绍了PCI和PCIe的硬 ...
- OA办公软件篇(一)—组织架构
OA办公软件篇(一)-组织架构 背景 作用 迭代历程 具体实现 写在最后 背景 在说组织架构之前,我们先来说说OA本身. 百度百科解释OA为:办公自动化(Office Automation,简称O ...
- OA办公软件篇(三)—审批流
背景 作用 迭代历程 具体实现 写在最后 背景 在前面两篇文章中,我们分别讲了组织架构和权限管理,今天我们来讲一个跟组织架构关系比较密切的功能-审批流. 审批流,通俗来说就是一个完整的审批流程,是 ...
- iOS系列 基础篇 04 探究视图生命周期
iOS系列 基础篇 04 探究视图生命周期 视图是应用的一个重要的组成部份,功能的实现与其息息相关,而视图控制器控制着视图,其重要性在整个应用中不言而喻. 以视图的四种状态为基础,我们来系统了解一下视 ...
- 智能家居-3.基于esp8266的语音控制系统(软件篇)
智能家居-1.基于esp8266的语音控制系统(开篇) 智能家居-2.基于esp8266的语音控制系统(硬件篇) 智能家居-3.基于esp8266的语音控制系统(软件篇) 赞赏支持 QQ:505645 ...
- OA办公软件篇(二)—权限管理
权限管理的背景 权限管理的作用 迭代历程 关键名词释义 权限管理模型 具体实现 写在最后 权限管理的背景 在OA办公软件篇(一)-组织架构一文中,我们说到组织架构是软件系统的权限体系的重要搭建依据 ...
随机推荐
- RabbitMQ(一)安装篇
1. RabbitMQ 的介绍➢ 什么是 MQ?MQ 全称为 Message Queue, 消息队列(MQ)是一种应用程序对应用程序的通信方法.➢ 要解决什么样的问题?在项目中,将一些无需即时返回且耗 ...
- Kubernetes-5.Pod资源控制器(1)
docker version:20.10.2 kubernetes version:1.20.1 本文概述Kubernetes Pod资源控制器的ReplicaSet.Deployment.Daemo ...
- 研究了一下 Webpack 打包原理,顺手挣了个 AirPods Pro
这些年,Webpack 基本成了前端项目打包构建的标配.关于它的原理和用法的文章在网上汗牛充栋,大家或多或少都看过一些.我也一样,大概了解过它的构建过程以及常用 loader 和 plugin 的配置 ...
- 小心你的个人信息——GitHub 热点速览 v.21.09
作者:HelloGitHub-小鱼干 浏览过必有痕迹,有什么可以抹去社交痕迹的方法呢?social-analyzer 是一个可在 350+ 网站分析特定用户资料的工具,你可以用它来"人肉&q ...
- 定西+简单dp
定西 ECNU-3531 #include<iostream> #include<cstdio> #include<algorithm> #include<c ...
- LeetCode-祖父节点值为偶数的结点值之和
祖父节点值为偶数的结点值之和 LeetCode-1315 这题稍微难度有点大,但是仔细思考还是可以找到思路的. 因为只需要找到祖父节点这最上两层,所以可以带一个参数记录一下祖父节点是否是偶数,以及父节 ...
- JS(ES6)、Vue.js、node.js
JS行为(ESMAScript, JSdom, bom)$.ajax() <- (xmlhttpRequest由这个封装来的) -> axios(vue版) = ajax技术jque ...
- DRF(django rest-framework)
1.什么是DRF django组件,快速帮助我们开发遵循restful规范的一个组件 2.什么是restful规范 RESTful的URL用于指定资源,URL中只能使用名词的组合来标识资源," ...
- 2018.12-2019.1 TO-DO LIST
AC自动机 P3808 [模板]AC自动机(简单版)(完成时间:2018.12.06) P3796 [模板]AC自动机(加强版)(完成时间:2018.12.06) P2444 [POI2000]病毒( ...
- 2019HDU多校第一场 6582 Path 【最短路+最大流最小割】
一.题目 Path 二.分析 首先肯定要求最短路,然后如何确定所有的最短路其实有多种方法. 1 根据最短路,那么最短路上的边肯定是可以满足$dist[from] + e.cost = dist[to] ...