XTDRONE：ego_planner三维运动规划

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

; 一、ego_planner功能包

1. ego_planner_node.cpp

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

1. 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()

1. 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话题

"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话题

"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">

    <remap from="~odom_world" to="/$(arg odometry_topic)"/>

    <remap from="~grid_map/pose"   to = "/iris_$(arg drone_id)/$(arg camera_pose_topic)"/>

    <remap from="~grid_map/depth" to = "/iris_$(arg drone_id)/$(arg depth_topic)"/>

    <remap from="~grid_map/odom" to="/xtdrone/iris_$(arg drone_id)/$(arg odometry_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);

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

Original: https://blog.csdn.net/dueen1123/article/details/124057259
Author: dueen1123
Title: XTDRONE：ego_planner三维运动规划