• Jetson Orin NX 开发指南（7）: EGO-Swarm 的编译与运行

    一、前言

    EGO-Planner 浙江大学 FAST-LAB 实验室的开源轨迹规划算法是，受到 IEEE Spectrum 等知名科技媒体的报道，其理论技术较为前沿，是一种不依赖于ESDF，基于B样条的规划算法，并且规划成功率、算法消耗时间、代价数值等性能方面都要高于其他几种知名算法。

    而 EGO-Swarm 是基于 EGO-Planner 拓展的去中心化的无人机集群算法，有助于智能小车或自主无人机集群的规划的学习与开发

    由于 EGO-Planner 是 EGO-Swarm 的一部分，并且他们的安装其实差别不大，因此本文主要介绍 EGO-Swarm 的编译与运行，参考

    https://github.com/ZJU-FAST-Lab/ego-planner-swarm

    https://github.com/ZJU-FAST-Lab/ego-planner

    GitHub - ZJU-FAST-Lab/Fast-Drone-250: hardware and software design of the 250mm autonomous drone

    由于 Jetson 系列开发板常用于当作机载电脑，因此本文介绍如何在 Jetson Orin NX 开发板上编译和运行 EGO-Swarm，当然本文对 EGO-Planner 同样适用。

    二、编译 EGO-Swarm

    首先安装依赖

    sudo apt-get install libarmadillo-dev

    然后创建并进入工作空间

    1. mkdir -p ~/catkin_ws/src/
    2. cd ~/catkin_ws/src/

    从 GitHub 上下载 EGO-Swarm 源码

    git clone https://github.com/ZJU-FAST-Lab/ego-planner-swarm.git

    进入 EGO-Swarm 工作空间并编译

    1. cd ~/catkin_ws/src/ego-planner-swarm
    2. catkin_make

    编译完成显示如下结果

    三、运行 EGO-Swarm

    接下来我们运行 EGO-Swarm，主要分为仿真和实验两个部分

    3.1 EGO-Swarm 仿真

    首先通过快捷键 ctrl + alt + A 打开超级终端，如果没有安装则参考下文安装

    Jetson Orin NX 开发指南（2）: 基本环境配置_想要个小姑娘的博客-CSDN博客

    将超级终端划分为两个终端，全选后 source 一下工作空间，终端输入

    source ~/catkin_ws/src/ego-planner-swarm/devel/setup.bash

    在第一个终端输入

    roslaunch ego_planner rviz.launch

    在第二个终端输入

    roslaunch ego_planner swarm.launch

    如下所示

    依次执行可以得到如下结果

    至此，EGO-Swarm 的仿真运行就实现了！

    3.2 EGO-Swarm 实验

    由于条件有限，我么在此仅仅只是将 EGO-Swarm 与 VINS-Fusion 进行连接，并且这里只涉及单个无人机（在这种情况下 EGO-Swarm 与 EGO-Planner 是等价的），其中 EGO-Swarm 单个无人机通过 VINS-Fusion 来获取里程计信息，同时通过深度相机数据来获取周围环境的情况。

    3.2.1 创建文件配置

    首先需要配置实验用的一些参数，对应于仿真中的 advanced_param.xml 文件，

    其次需要配置调用 VINS-Fusion 里程计信息和 Realsense 深度相机信息的 launch 启动文件，对应于仿真中的 single_run_in_sim.launch 文件，

    此外还需要用于可视化的 rviz 文件，对应于仿真中的 default.rviz 文件，

    具体的配置可以参考 FAST-LAB 实验室的 fast-drone-250 中采用的 xml 和 launch 文件：

    GitHub - ZJU-FAST-Lab/Fast-Drone-250: hardware and software design of the 250mm autonomous drone

    主要是如下三个文件

    但是由于 VINS-Fusion 中里程计发布的话题是 /vins_estimator/imu_propagate，而上面订阅的话题是 /vins_fusion/imu_propagate，这是因为在 fast-drone-250 中，vins_estimator 节点被重命名为了 vins_fusion，从而产生了差异，只需修改以下订阅的话题即可，其余的可以不做修改，（当然如果标定了相机内外参以及 imu 的噪声，则可以将之后的数据修改上去，VINS-Fusion 或者 EGO-Swarm 上都要修改），具体操作如下

    在 ~/catkin_ws/src/ego-planner-swarm/src/planner/plan_manage/launch/ 路径下创建四个文件

    （1）advanced_param_exp.xml，其内容如下

    1. <launch>
    2.   <arg name="map_size_x_"/>
    3.   <arg name="map_size_y_"/>
    4.   <arg name="map_size_z_"/>
    5.  
    6.   <arg name="odometry_topic"/>
    7.   <arg name="camera_pose_topic"/>
    8.   <arg name="depth_topic"/>
    9.   <arg name="cloud_topic"/>
    10.  
    11.   <arg name="cx"/>
    12.   <arg name="cy"/>
    13.   <arg name="fx"/>
    14.   <arg name="fy"/>
    15.  
    16.   <arg name="max_vel"/>
    17.   <arg name="max_acc"/>
    18.   <arg name="planning_horizon"/>
    19.  
    20.   <arg name="point_num"/>
    21.   <arg name="point0_x"/>
    22.   <arg name="point0_y"/>
    23.   <arg name="point0_z"/>
    24.   <arg name="point1_x"/>
    25.   <arg name="point1_y"/>
    26.   <arg name="point1_z"/>
    27.   <arg name="point2_x"/>
    28.   <arg name="point2_y"/>
    29.   <arg name="point2_z"/>
    30.   <arg name="point3_x"/>
    31.   <arg name="point3_y"/>
    32.   <arg name="point3_z"/>
    33.   <arg name="point4_x"/>
    34.   <arg name="point4_y"/>
    35.   <arg name="point4_z"/>
    36.  
    37.   <arg name="flight_type"/>
    38.   <arg name="use_distinctive_trajs"/>
    39.  
    40.   <arg name="obj_num_set"/>
    41.  
    42.   <arg name="drone_id"/>
    43.  
    44.  
    45.   <!-- main node -->
    46.   <!-- <node pkg="ego_planner" name="ego_planner_node" type="ego_planner_node" output="screen" launch-prefix="valgrind"> -->
    47.   <node pkg="ego_planner" name="drone_$(arg drone_id)_ego_planner_node" type="ego_planner_node" output="screen">
    48.   
    49.     <remap from="~odom_world" to="$(arg odometry_topic)"/>
    50.     <remap from="~planning/bspline" to = "/drone_$(arg drone_id)_planning/bspline"/>
    51.     <remap from="~planning/data_display" to = "/drone_$(arg drone_id)_planning/data_display"/>
    52.     <remap from="~planning/broadcast_bspline_from_planner" to = "/broadcast_bspline"/>
    53.     <remap from="~planning/broadcast_bspline_to_planner" to = "/broadcast_bspline"/>
    54.  
    55.     <remap from="~grid_map/odom" to="$(arg odometry_topic)"/>
    56.     <remap from="~grid_map/cloud" to="$(arg cloud_topic)"/>
    57.     <remap from="~grid_map/pose"   to = "$(arg camera_pose_topic)"/> 
    58.     <remap from="~grid_map/depth" to = "$(arg depth_topic)"/>
    59.     
    60.  
    61.     <!-- planning fsm -->
    62.     <param name="fsm/flight_type" value="$(arg flight_type)" type="int"/>
    63.     <param name="fsm/thresh_replan_time" value="1.0" type="double"/>
    64.     <param name="fsm/thresh_no_replan_meter" value="1.0" type="double"/>
    65.     <param name="fsm/planning_horizon" value="$(arg planning_horizon)" type="double"/> <!--always set to 1.5 times grater than sensing horizen-->
    66.     <param name="fsm/planning_horizen_time" value="3" type="double"/>
    67.     <param name="fsm/emergency_time" value="1.0" type="double"/>
    68.     <param name="fsm/realworld_experiment" value="true"/>
    69.     <param name="fsm/fail_safe" value="true"/>
    70.  
    71.     <param name="fsm/waypoint_num" value="$(arg point_num)" type="int"/>
    72.     <param name="fsm/waypoint0_x" value="$(arg point0_x)" type="double"/>
    73.     <param name="fsm/waypoint0_y" value="$(arg point0_y)" type="double"/>
    74.     <param name="fsm/waypoint0_z" value="$(arg point0_z)" type="double"/>
    75.     <param name="fsm/waypoint1_x" value="$(arg point1_x)" type="double"/>
    76.     <param name="fsm/waypoint1_y" value="$(arg point1_y)" type="double"/>
    77.     <param name="fsm/waypoint1_z" value="$(arg point1_z)" type="double"/>
    78.     <param name="fsm/waypoint2_x" value="$(arg point2_x)" type="double"/>
    79.     <param name="fsm/waypoint2_y" value="$(arg point2_y)" type="double"/>
    80.     <param name="fsm/waypoint2_z" value="$(arg point2_z)" type="double"/>
    81.     <param name="fsm/waypoint3_x" value="$(arg point3_x)" type="double"/>
    82.     <param name="fsm/waypoint3_y" value="$(arg point3_y)" type="double"/>
    83.     <param name="fsm/waypoint3_z" value="$(arg point3_z)" type="double"/>
    84.     <param name="fsm/waypoint4_x" value="$(arg point4_x)" type="double"/>
    85.     <param name="fsm/waypoint4_y" value="$(arg point4_y)" type="double"/>
    86.     <param name="fsm/waypoint4_z" value="$(arg point4_z)" type="double"/>
    87.  
    88.     <param name="grid_map/resolution"      value="0.15" /> 
    89.     <param name="grid_map/map_size_x"   value="$(arg map_size_x_)" /> 
    90.     <param name="grid_map/map_size_y"   value="$(arg map_size_y_)" /> 
    91.     <param name="grid_map/map_size_z"   value="$(arg map_size_z_)" /> 
    92.     <param name="grid_map/local_update_range_x"  value="5.5" /> 
    93.     <param name="grid_map/local_update_range_y"  value="5.5" /> 
    94.     <param name="grid_map/local_update_range_z"  value="4.5" /> 
    95.     <param name="grid_map/obstacles_inflation"     value="0.299" /> 
    96.     <param name="grid_map/local_map_margin" value="10"/>
    97.     <param name="grid_map/ground_height"        value="-0.01"/>
    98.     <!-- camera parameter -->
    99.     <param name="grid_map/cx" value="$(arg cx)"/>
    100.     <param name="grid_map/cy" value="$(arg cy)"/>
    101.     <param name="grid_map/fx" value="$(arg fx)"/>
    102.     <param name="grid_map/fy" value="$(arg fy)"/>
    103.     <!-- depth filter -->
    104.     <param name="grid_map/use_depth_filter" value="true"/>
    105.     <param name="grid_map/depth_filter_tolerance" value="0.15"/>
    106.     <param name="grid_map/depth_filter_maxdist"   value="5.0"/>
    107.     <param name="grid_map/depth_filter_mindist"   value="0.2"/>
    108.     <param name="grid_map/depth_filter_margin"    value="2"/>
    109.     <param name="grid_map/k_depth_scaling_factor" value="1000.0"/>
    110.     <param name="grid_map/skip_pixel" value="2"/>
    111.     <!-- local fusion -->
    112.     <param name="grid_map/p_hit"  value="0.65"/>
    113.     <param name="grid_map/p_miss" value="0.35"/>
    114.     <param name="grid_map/p_min"  value="0.12"/>
    115.     <param name="grid_map/p_max"  value="0.90"/>
    116.     <param name="grid_map/p_occ"  value="0.80"/>
    117.     <param name="grid_map/min_ray_length" value="0.3"/>
    118.     <param name="grid_map/max_ray_length" value="5.0"/>
    119.  
    120.     <param name="grid_map/visualization_truncate_height"   value="1.8"/>
    121.     <param name="grid_map/show_occ_time"  value="false"/>
    122.     <param name="grid_map/pose_type"     value="2"/>  
    123.     <param name="grid_map/frame_id"      value="world"/>
    124.  
    125.   <!-- planner manager -->
    126.     <param name="manager/max_vel" value="$(arg max_vel)" type="double"/>
    127.     <param name="manager/max_acc" value="$(arg max_acc)" type="double"/>
    128.     <param name="manager/max_jerk" value="4" type="double"/>
    129.     <param name="manager/control_points_distance" value="0.4" type="double"/>
    130.     <param name="manager/feasibility_tolerance" value="0.05" type="double"/>
    131.     <param name="manager/planning_horizon" value="$(arg planning_horizon)" type="double"/>
    132.     <param name="manager/use_distinctive_trajs" value="$(arg use_distinctive_trajs)" type="bool"/>
    133.     <param name="manager/drone_id" value="$(arg drone_id)"/>
    134.  
    135.   <!-- trajectory optimization -->
    136.     <param name="optimization/lambda_smooth" value="1.0" type="double"/>
    137.     <param name="optimization/lambda_collision" value="0.5" type="double"/>
    138.     <param name="optimization/lambda_feasibility" value="0.1" type="double"/>
    139.     <param name="optimization/lambda_fitness" value="1.0" type="double"/>
    140.     <param name="optimization/dist0" value="0.5" type="double"/>
    141.     <param name="optimization/swarm_clearance" value="0.5" type="double"/>
    142.     <param name="optimization/max_vel" value="$(arg max_vel)" type="double"/>
    143.     <param name="optimization/max_acc" value="$(arg max_acc)" type="double"/>
    144.  
    145.     <param name="bspline/limit_vel" value="$(arg max_vel)" type="double"/>
    146.     <param name="bspline/limit_acc" value="$(arg max_acc)" type="double"/>
    147.     <param name="bspline/limit_ratio" value="1.1" type="double"/>
    148.  
    149.   <!-- objects prediction -->
    150.     <param name="prediction/obj_num" value="$(arg obj_num_set)" type="int"/>
    151.     <param name="prediction/lambda" value="1.0" type="double"/>
    152.     <param name="prediction/predict_rate" value="1.0" type="double"/>
    153.   
    154.   
    155.  
    156.  
    157.   </node>
    158.  
    159. </launch>

    （2）single_run_in_exp.launch，其内容如下

    1. <launch>
    2.     <!-- number of moving objects -->
    3.     <arg name="obj_num" value="10" />
    4.     <arg name="drone_id" value="0"/>
    5.  
    6.     <arg name="map_size_x" value="100"/>
    7.     <arg name="map_size_y" value="50"/>
    8.     <arg name="map_size_z" value="3.0"/>
    9.     <arg name="odom_topic" value="/vins_estimator/imu_propagate"/>
    10.     
    11.     <!-- main algorithm params -->
    12.     <include file="$(find ego_planner)/launch/advanced_param_exp.xml">
    13.         <arg name="drone_id" value="$(arg drone_id)"/>
    14.         <arg name="map_size_x_" value="$(arg map_size_x)"/>
    15.         <arg name="map_size_y_" value="$(arg map_size_y)"/>
    16.         <arg name="map_size_z_" value="$(arg map_size_z)"/>
    17.         <arg name="odometry_topic" value="$(arg odom_topic)"/>
    18.         <arg name="obj_num_set" value="$(arg obj_num)" />
    19.         <!-- camera pose: transform of camera frame in the world frame -->
    20.         <!-- depth topic: depth image, 640x480 by default -->
    21.         <!-- don't set cloud_topic if you already set these ones! -->
    22.         
    23.         
    24.         
    25.