程序员经验分享-hwhale

XTDRONE：ego_planner三维运动规划

2023-05-16

ros常用消息类型：
https://blog.csdn.net/xhtchina/article/details/119707553

请添加图片描述
iris_0_ego_transfer话题在~/XTDrone/motion_planning/3d/ego_transfer.py 被转化成iris_0/camera_pose

一、ego_planner功能包

1. ego_planner_node.cpp

注册ROS节点，实例化EGOReplanFSM类对象并进行初始化，程序入口

2. ego_replan_fsm.cpp

ExecFSMCallback()

这个函数的触发时间是每0.01秒。首先，每1秒打印一次当前执行状态。然后，根据执行状态变量 exec_state_进入switch循环。

如果状态是INIT（初始化），则判断有没有收到odometry 和 plan的 ttrigger是否被触发。 have_odom 和 trigger 这两个bool变量分别在odometryCallback()和waypointCallback()这两个消息订阅的回调函数中被改变。如果都通过，改变当前状态为WAIT_TARGET,并跳出当前循环。

如果状态是GEN_NEW_TRAJ(生成轨迹)，则调用 planFromGlobalTraj() 函数进行规划。成功则改变执行状态为EXEC_TRAJ，执行 publishSwarmTrajs()；失败则改变执行状态为GEN_NEW_TRAJ。这里我们来看 planFromGlobalTraj 函数。如果规划成功的话，就通过Bspline_pub把相应的B样条轨迹发布出去，否则返回FALSE

如果状态为REPLAN_TRAJ(重新规划)，则利用当前的位置，速度，状态，进行 planFromCurrentTraj() 重规划，如果成功，则将执行状态更改为EXEC_TRAJ；否则将执行状态更改为REPLAN_TRAJ

如果状态为EXEC_TRAJ(执行轨迹)，则判断是否需要进行重规划。如果没有达到end pointt，如果当前与终点距离 < 不需要重规划的阈值，则执行下一个路标点；如果当前时间距离起始时间已经超过当前执行轨迹的时长，则将have_target置于false，将执行状态变为WAIT_TARGET；反之，则进入重规划阶段REPLAN_TRAJ

请添加图片描述

CheckCollisionCallback()

判断目标点是否有障碍物 或者 是轨迹执行过程中是否有障碍物的回调函数

先判断轨迹中是否有障碍物，如果有障碍物，即occ为真，如果可以成功执行 planFromCurrentTraj()，即轨迹规划成功，则状态转为EXEC_TRAJ；如果轨迹规划不成功并且时间允许，则状态转为REPLAN_TRAJ；否则停止

（如果目标点有障碍物，就在目标点周围通过离散的半径及角度循环来寻找新的安全的目标点。如果找到了，就直接改变状态进入REPLAN_TRAJ；如果目标点周围没有障碍物且目前状态是EXEX_TRAJ，则利用planner_manager的 checkTrajCollision 函数进行轨迹检查，如果轨迹不发生碰撞，则无事发生，如果轨迹碰撞，则状态变为REPLAN_TRAJ，进行重规划）

PlanFromGlobalTraj()

传入时间，判断是否能生成全局轨迹，调用callReboundReplan()函数中的planner_manager_->reboundReplan()

PlanFromCurrentTraj()

判断是否能生成当前轨迹？调用callReboundReplan()函数中的planner_manager_->reboundReplan()

3. planner_manager.cpp

PlanGlobalTraj( )

在planner_manager.cpp中，检查路径规划是否成功

ReboundReplan()

先初始化轨迹，利用mini-snap的方式完成，或者从之前的轨迹再生成轨迹；接着优化轨迹；最后重新调整时间段分配比例

论文中的算法流程
请添加图片描述

planGlobalTrajWaypoints() & planGlobalTraj()

前者是规划具有起点和一些路标点的全局路径，后者是规划具有起点和目标殿两个点的全局路径
前者没有被调用；后者在ego_replan_fsm.cpp中的planNextWaypoint()函数被调用

二、启动文件

请添加图片描述

single_uav.launch

(1) 创建iris_0_map_to_world节点，发布/tf话题

<node pkg="tf" type="static_transform_publisher" name="iris_0_map_to_world"
    args="0.0 0.0 0 0.0 0.0 0.0 /map /world 40" />

node的属性分别为：pkg程序包名字、type可执行文件的名字、name节点名字、args参数。
args前三个参数为x y z，分别代表着相应轴的平移，单位是米 ；
args中间三个参数为qx qy qz，对应yaw pitch roll 分别代表着绕三个轴的转动，单位是弧度 ；
args的 /map /world 分别代表父坐标系和子坐标系；
args最后一个参数40为发布频率，单位为毫秒

(2) 创建iris_0_world_to_ground_plane节点，发布/tf话题

<node pkg="tf" type="static_transform_publisher" name="iris_0_world_to_ground_plane"
    args="0.0 0.0 0 0.0 0.0 0.0 /world /ground_plane 40" />

(3) 启动run_in_xtdrone.launch文件

run_in_xtdrone.launch

(1) 启动advanced_param_xtdrone.xml文件
在xml文件中启动ego_planner_node可执行文件，生成iris_0_ego_planner_node节点
其中有关odometry /camera_pose /image_raw都是在ego_planner_node中有关地图的句柄中订阅的

  <node pkg="ego_planner" name="iris_$(arg drone_id)_ego_planner_node" type="ego_planner_node" output="screen">
    #订阅的话题：
    #vins_estimator/odometry
    <remap from="~odom_world" to="/$(arg odometry_topic)"/> 
    #iris_0/camera_pose
    <remap from="~grid_map/pose"   to = "/iris_$(arg drone_id)/$(arg camera_pose_topic)"/> 
    #iris_0/realsense/depth_camera/depth/image_raw
    <remap from="~grid_map/depth" to = "/iris_$(arg drone_id)/$(arg depth_topic)"/> 
    #/xtdrone/iris_0/vins_estimator/odometry
    <remap from="~grid_map/odom" to="/xtdrone/iris_$(arg drone_id)/$(arg odometry_topic)"/>
    #/iris_0/$(arg cloud_topic)
    <remap from="~grid_map/cloud" to="/iris_$(arg drone_id)/$(arg cloud_topic)"/>
    <remap from="~planning/broadcast_bspline_from_planner" to = "/broadcast_bspline"/> 
   
    #发布的话题：
    <remap from="~planning/bspline" to = "/xtdrone/iris_$(arg drone_id)/planning/bspline"/>
    <remap from="~planning/broadcast_bspline_to_planner" to = "/broadcast_bspline"/>

(2) 启动traj_server的可执行文件，对应traj_server.cpp文件，生成iris_0_traj_server节点

<node pkg="ego_planner" name="iris_$(arg drone_id)_traj_server" type="traj_server" output="screen">
    <remap from="position_cmd" to="/xtdrone/iris_$(arg drone_id)/planning/pos_cmd"/>
    <remap from="pose_cmd" to="/xtdrone/iris_$(arg drone_id)/cmd_pose_enu"/>
    <remap from="~planning/bspline" to="/xtdrone/iris_$(arg drone_id)/planning/bspline"/>
    <param name="traj_server/time_forward" value="1.0" type="double"/>
  </node>

三、Map地图

在planner_manager.cpp创建地图句柄，该句柄连接grid_map.cpp文件，接受各种传感器信息，发布障碍物位置信息

grid_map_.reset(new GridMap);
grid_map_->initMap(nh);# ego_planner/plan_env/src/grid_map.cpp

grid_map.cpp完成功能：
（/vins_estimator/extrinsic，相机到IMU的外参）
1. 订阅：
订阅grid_map/depth，即depth_camera/depth/image_raw
订阅vins_estimator/extrinsic
订阅grid_map/pose，即camera_pose
订阅grid_map/odom，
2. 发布：
发布grid_map/occupancy，即occupancy_buffer中的障碍物的位置
发布grid_map/occupancy_inflate，即occupancy_buffer_inflate中的障碍物的位置
其中，occupancy_buffer及occupancy_buffer_inflate容器中局部地图范围内的所有点进行判断，若occupancy_buffer超过障碍物最小概率，同时occupancy_buffer_inflate数为1，且不超过高度范围，则将其从voxel序列还原成三维位置点，推入cloud容器中，最后一并发布。

grid_map.cpp代码解读：
https://blog.csdn.net/weixin_42284263/article/details/122283727?utm_medium=distribute.pc_relevant.none-task-blog-2defaultbaidujs_title~default-0.pc_relevant_default&spm=1001.2101.3001.4242.1&utm_relevant_index=3

depthPoseCallback()

获得最新的相机Pose 与 深度图，如果相机的位置处于全局地图Map_size之外，则就会将md.occ_need_update这一flag置false，反之置为true。

updateOccupancyCallback()

occ_timer_ = node_.createTimer(ros::Duration(0.05), &GridMap::updateOccupancyCallback, this);

定时器回调函数，地图节点通过这一回调函数定时更新地图。

第一个flag是md.occ_need_update，只有接收到新图像且位于地图范围之内，才会进行接下来的projectDepthImage()及raycastProcess()这两个流程。

第二flag是md.local_updated，这一flag只在raycastProcess中判断深度图投影点数量不为零是才会置为true，这时才会进入clearAndInflateLocalMap()这一流程，对局部地图进行膨胀和更新。

详细代码解读：https://blog.csdn.net/weixin_45736684/article/details/114239477

四、B样条轨迹

在planner_manager.cpp创建句柄如下

bspline_optimizer_.reset(new BsplineOptimizer);
bspline_optimizer_->setParam(nh);
bspline_optimizer_->setEnvironment(grid_map_, obj_predictor_);

grid_map_包括所有GridMap类里的信息，例如mp_，md_

五、搜索

在planner_manager.cpp调用搜索部分代码

bspline_optimizer_->a_star_.reset(new AStar);
bspline_optimizer_->a_star_->initGridMap(grid_map_, Eigen::Vector3i(100, 100, 100));

本文参考：
https://blog.csdn.net/qq_35635374/article/details/121610987
https://zhuanlan.zhihu.com/p/369198624
https://wenku.baidu.com/view/befa33eaa2c7aa00b52acfc789eb172ded6399fa.html
https://zhuanlan.zhihu.com/p/369196042

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

XTDRONE：ego_planner三维运动规划 的相关文章

  • 质心跟踪算法

    质心跟踪算法依赖于 xff08 1 xff09 现有对象质心 xff08 即 xff0c 质心跟踪器之前已经看到的对象 xff09 与 xff08 2 xff09 视频中后续帧之间的新对象质心之间的欧几里得距离 质心跟踪算法的主要假设是一个

  • 为什么我要刷leetcode!

    从今天开始我会每天坚持刷leetcode 为什么要这么做呢 xff1f 其实也是闲的哈哈哈哈 xff0c 被病毒困在家里那里也去不了 xff0c 那就不如来刷代码吧 xff01 其实不管是C 43 43 还是C还是java等各种各样的计算机

  • 标准模板库学习（5）----算法之非修正序列算法

    算法是STL的中枢 xff0c STL提供了算法库 xff0c 算法库都是模板函数 xff0c 主要分为四类 xff0c 非修正序列算法 修正序列算法 排序算法和数值算法 本文介绍非修正序列算法 adjacent find start en

  • Ubuntu中apt update和upgrade的区别

    原文链接 xff1a https blog csdn net CSDN duomaomao article details 77802673 简要说明 xff1a apt update xff1a 只检查 xff0c 不更新 xff08 已

  • Java中的信号量（Semaphore）

    初识Semaphore 信号量 xff0c 也可以称其为 信号灯 xff0c 它的存在就如同生活中的红绿灯一般 xff0c 用来控制车辆的通行 在程序员眼中 xff0c 线程就好比行驶的车辆 xff0c 程序员就可以通过信号量去指定线程是否

  • USB 2.0_ser!或者U232-P9 型USB转串Win7 32位或64位驱动 以及 USB转串串口序号查看和设置

    前几天叫同事在电脑城买了个USB转串数据线 xff0c 但是回来后在网上找了很多驱动都不行 觉得这个问题花了不少时间的 xff0c 我也拆开了 xff0c 打算按照芯片型号找驱动 xff0c 但是看不到芯片型号 现在终于找到合适的了 把这个

  • 《Java核心技术卷1》

    第3章 Java的基础程序设计结构 整型 用int类型表示一百万可以这么写 xff08 since 1 7 xff09 span class token keyword int span a span class token operato

  • voxl-flight quick start

    voxl flight 官方地址 xff1a https www modalai com 硬件及接口 两个版本 Snapdragon 821 xff1a 四核最高2 15GH xff0c GPU xff0c 2xDSP 视频支持 xff1a

  • 零基础如何学习优达学城的《无人驾驶入门》？

    因为感兴趣 xff0c 而且看好无人驾驶行业 xff0c 我学习了优达学城的 无人驾驶入门 课程 最近整理了无人驾驶领域的资料 xff0c 写成文章分享给大家 作为系列文章的第一篇 xff0c 我想介绍一下 无人驾驶入门 这门课 xff0c

  • realsense D435 标定(calibration)

    realsense D435 标定 文章目录 realsense D435 标定1 确定是否需要标定设备信息步骤操作打印标定目标开启标定程序 校正结果展示比较 文档 1 确定是否需要标定 工具 Depth Quality Tool 要求 将

  • 如何在linux执行PHP文件

    1 刚导入到linux系统中文件是没有可执行权 2 首先赋予文件可执行权限 chmod 43 x 文件名 例如 xff1a chomd 43 x czrkdjb php 如果要用 czrkdjb php执行 xff0c 需要在czrkdjb

  • 阿里P8大佬亲自讲解！写给程序员的Flutter详细教程，灵魂拷问

    我们程序员经常迷茫于有太多东西要学 xff0c 有些找不到方向 不知所措 很多程序员都愿意说 xff0c 我想变得更好 xff0c 但是更好是什么却很模糊 xff0c 同时我们又不知道该怎么样去做 我们的生命如此短暂 xff0c 作为程序员

  • VSCode 连接远程服务器使用图形化界面（GUI）

    1 基本环境 本地电脑系统 xff1a window10 远程服务器系统 xff1a Ubuntu18 04 2 VSCode版本 xff1a 1 51 1 2 问题描述 vscod提供了优秀的远程连接插件 xff08 Remote SSH

  • curl 命令大全及常用实例

    一 xff0c curl命令参数 a append 上传文件时 xff0c 附加到目标文件 A user agent lt string gt 设置用户代理发送给服务器 anyauth 可以使用 任何 身份验证方法 b cookie lt

  • 关于putty不能连接虚拟机的问题解决

    一 首先需要ping 一下 xff0c 看是否能ping通 xff0c 如果正常进入下一步 二 进入虚拟机终端输入 ssh localhost 如果提示 ssh connect to host localhost port 22 Conne

  • postman进行http接口测试

    HTTP的接口测试工具有很多 xff0c 可以进行http请求的方式也有很多 xff0c 但是可以直接拿来就用 xff0c 而且功能还支持的不错的 xff0c 我使用过的来讲 xff0c 还是postman比较上手 优点 xff1a 1 支

  • Linux系统迁移（将配置好的系统安装到其它电脑上）

    Linux系统迁移 说在前面 xff1a 下面有几个教程链接 xff0c 我都是通过这几个链接来完成的系统备份与系统恢复 并且遇到过一些问题 xff0c 踩过一些坑 建议先看完我的说明再进行操作 xff0c 少走弯路 没有图是因为下面分享的

  • 搭建 公网FTP服务器 外网访问

    我是在ubuntu 20 04 上配置的 xff0c 需要用到公网IP 没有公网IP的 xff0c 可以考虑花生壳这类应用来做内网穿透 1 配置FTP服务器 安装vsftpd sudo apt install vsftpd sudo vim

  • clang-format配置与使用

    clang format配置与使用 参考教程 1 安装 下载clang format xff0c 设置环境变量 我使用的是vscode扩展中的clang format 位于 xff1a extensions ms vscode cpptoo

  • AI一般是用来制作什么的

    AI一般用来制作logo 分页 xff0c 海报等等 面板堆栈的话就是很多功能堆放的位置 一般打印出来的话用cmyk模式 如果是在web端的话用RGB模式 xff0c 因为cmyk模式在你进行存储的过程中颜色可能会丢失 出血值就是在你打印东

随机推荐

  • Eclipse安装插件的三种方式

    本文介绍Eclipse插件的安装方法 Eclipse插件的安装方法大体有三种 xff1a 直接复制 使用link文件 xff0c 以及使用eclipse自带的图形界面的插件安装方法 AD xff1a 做为当下最流行的开源IDE之一 xff0

  • 获取realsense内参

    文章目录 xff11 xff0e 无畸变 xff12 xff0e Brown Conrady 畸变模型3 Opencv 里畸变图像矫正过程 xff11 xff0e 无畸变 相机内参包括相机矩阵参数和畸变系数 相机内参矩阵为3 3的矩阵 xf

  • 十年研发经验工程师的嵌入式学习书籍大推荐

    从事嵌入式研发行业十年 xff0c 认为学习就是要不断的吸纳知识 xff0c 在研发过程中 xff0c 经常会遇到一些问题 xff0c 这种发现问题并解决问题的过程就是进步 为什么选择学习嵌入式 xff1f 嵌入式系统无疑是当前最热门最有发

  • js手机号正则表达式验证

    看到网上很多代码 都很复杂 xff0c 还包括以中文开头的86 xff0c 17951 xff0c 其实谁会填这么多 xff0c 无非是检验一下他们是否位数对不对 xff0c 开头有没有写错而已 下面我们从百度百科的手机号码历程来看 xff

  • 正则验证匹配中文姓名全部源字符串

    这个是验证匹配中文姓名的全部源串 xff0c 在网上找了很久 xff0c 大都是验证匹配含有中文 xff0c 就在网上某人提供的正则的基础上修改成了验证所填姓名的每个字符 xff0c 只有都匹配才能验证通过 该正则为 xff1a u4e00

  • 域名，网站名和URL区别

    要写一个正则表达式来验证输入域名是否有正确 xff0c 一直以为例如http www baidu com类似于这种才是网站域名 xff0c 经过百度才发现自己的认知是错误的 以下转载于百度经验 xff1a http jingyan baid

  • 23种设计模式(1)-Facade设计模式

    前记 曾经我遇见的一个需求是这样的 xff0c 接口A有个方法void methodA xff0c 类B需要实现接口A的methodA 方法 xff0c 并且在类B中需要把methodA 方法内部处理逻辑获得的结果利用C类实例的某个方法进行

  • Linux CentOS 7安装GNOME图形界面并设置默认启动方式

    为hadoop集群做准备 xff0c 没有多台电脑 xff0c 也只能委屈我这渣渣电脑了 在我的物理机上安装虚拟机 xff0c 再在虚拟机里面虚拟出两台电脑 xff0c 安装两个linux操作系统 1 xff0c 电脑太渣 xff0c 安装

  • dubbo学习资源

    简介 xff1a 项目有使用dubbo xff0c 以此需要学习一下 xff0c 搜集学习资源 一 xff0c 博客资源 1 xff0c dubbo学习 2 xff0c dubbo配置

  • 使用CSS3的box-sizing属性解决div宽高被内边距撑开的问题

    有时我们会给页面的元素 xff08 比如div xff09 设置个固定的高度或宽度 但如果给这个div又设置了内边距或者边框的话 xff0c 那么这个div就会被撑大 也就是其实际的尺寸变成了 xff1a 设置的宽高尺寸 43 内边距 43

  • 博客搬家：https://blog.csdn.net/u012995888

    本博客文章有些乱 xff0c 多有转载 xff0c 现在主要在另一CSDN账号更新原创文章 xff0c 点击查看博客主页 另外 xff0c 本博客后续可能会重新整理栏目 xff0c 更新高质量文章 xff0c 谢谢关注 xff01

  • C++ 常用设计模式(学习笔记)

    1 工厂模式 工厂模式 xff1a 简单工厂模式 工厂方法模式 抽象工厂模式 1 简单工厂模式 xff1a 主要特点是需要在工厂类中做判断 xff0c 从而创造相应的产品 xff0c 当增加新产品时 xff0c 需要修改工厂类 typede

  • 通过更新显卡驱动和内核，解决linux启动时在starting atd: [ok]停止的问题

    说得有些复杂 xff0c 你可以不用理会这些 xff0c 直接执行我罗列的那几个命令就行了 方法一 xff1a 网上有说In some cases the new install Gforce Drivers do not supporte

  • Streamedian/html5_rtsp_player接海康视频遇到的坑

    Streamedian是一套能够让浏览器免插件播放RTSP的项目 安装了其官方的server端后有一个demo 如图 xff0c 在输入处输入红框格式的RTSP地址 xff0c 如官方的demo地址 xff1a rtsp 184 72 23

  • 文档与笔记利器 reStructuredText 和 Sphinx

    原文http qixinglu com archives note tools restructuredtext sphinx 文档与笔记利器 reStructuredText 和 Sphinx 28六 2011 作者 投稿 转载 本文采用

  • 关于递归的问题：无限递归

    当你在写递归的时候 xff0c 可能会出现运行不出结果来 xff0c 此时你应该试着看看会不会出现无限递归的问题 xff0c 如果有的话 xff0c 你可以在前面增加一个判断语句 xff0c 满足条件进入执行相关操作与递归 xff0c 但在

  • 无人机路径规划1：orbslam2+VIO

    无人机路径规划1 xff1a orbslam2 43 VIO 安装XTDRONE平台 https www yuque com xtdrone manual cn basic config ros基本操作 ros中文版教程 http wiki

  • 无人机路径规划2 vins_fusion + rtab_map学习

    导航过程中的tf Map gt odom gt base footprint gt base link odom里程计 odom tf base footprint gt base link xff1a robot state publis

  • 无人机路径规划3：ego-planner三维运动规划实现

    XTDrone实现ego planner三维运动规划 编译ego palnner span class token function cp span r XTDrone motion planning 3d ego planner catk

  • XTDRONE：ego_planner三维运动规划

    ros常用消息类型 xff1a https blog csdn net xhtchina article details 119707553 iris 0 ego transfer话题在 XTDrone motion planning 3d

热门标签