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-2_defaultbaidujs_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

XTDRONE：ego_planner三维运动规划相关推荐

matlab人工势场法三维演示图,运动规划入门 | 5. 白话人工势场法，从原理到Matlab实现...
如何利用人工势场进行运动规划? 1.1 引力势场(Attractive Potential Field) 人工势场这个特殊的势场并不是一个单一的场,其实它是由两个场叠加组合而成的,一个是引力场,一个是 ...
走好每一步，基于C实现机器人运动学建模与标定、运动规划、轨迹规划算法
走好每一步,基于C实现机器人运动学建模与标定.运动规划.轨迹规划算法废话综述一:C部分初始C语言 Chapter2-4:基本数据类型 Chapter5-7:运算符.表达式.循环.分支与跳转 C ...
【APF三维路径规划】基于matlab人工势场算法无人机三维路径规划【含Matlab源码 168期】
一.获取代码方式获取代码方式1: 通过订阅紫极神光博客付费专栏,凭支付凭证,私信博主,可获得此代码. 获取代码方式2: 完整代码已上传我的资源:[三维路径规划]基于matlab人工势场算法无人机三维 ...
【RRT三维路径规划】基于matlab RRT算法无人机三维路径规划【含Matlab源码 155期】
一.获取代码方式获取代码方式1: 通过订阅紫极神光博客付费专栏,凭支付凭证,私信博主,可获得此代码. 获取代码方式2: 完整代码已上传我的资源:[三维路径规划]基于matlab RRT算法无人机三维 ...
六自由度机器人（机械臂）运动学建模及运动规划系列（二）——运动学分析
本篇主要介绍六轴机械臂的运动学分析. 运动学分析是工业机器人研究和应用的重要内容,是运动控制的基础,主要研究机器人末端坐标系与基坐标系的转换关系,分为正运动学和逆运动学分析两部分. 另外,对于刚刚学习 ...
【A_star三维路径规划】基于matlab A_star算法无人机山地三维路径规划【含Matlab源码 266期】
⛄一.A_star算法简介 0 引言随着现代技术的发展,飞行器种类不断变多,应用也日趋专一化.完善化,如专门用作植保的大疆PS-X625无人机,用作街景拍摄与监控巡察的宝鸡行翼航空科技的X8无人机, ...
机器人核心技术运动规划：让机器人想去哪就去哪
原文链接:机器人核心技术运动规划:让机器人想去哪就去哪! 前言 SLAM知识星球里经常有小伙伴问我,学习了SLAM开源代码后,如何进行导航.星球里我是这样回复的: 这里解释下运动规划和SLAM什么关系 ...
运动规划和SLAM什么关系？
原文链接:运动规划和SLAM什么关系? 前言 SLAM知识星球里经常有小伙伴问我,学习了SLAM开源代码后,如何进行导航.星球里我是这样回复的: 这里解释下运动规划和SLAM什么关系? 其实在企业里, ...
深蓝学院-机器人运动规划学习笔记-第一章
第一课移动机器人运动规划 Motion planning for mobile robots Introduction Course outline Typical planning methods ...
【GA三维路径规划】基于matlab遗传算法无人机三维路径规划【含Matlab源码 1268期】
一.无人机简介 0 引言随着现代技术的发展,飞行器种类不断变多,应用也日趋专一化.完善化,如专门用作植保的大疆PS-X625无人机,用作街景拍摄与监控巡察的宝鸡行翼航空科技的X8无人机,以及用作水下 ...

XTDRONE：ego_planner三维运动规划