基于fast-lio2来跑下ego-planner

odom订阅/Odometry话题就可以了

出现了，激光雷达实时是在动的，随着激光雷达的位置动了！！！！订阅激光SLAM的应该是OK的。

但是感觉激光雷达没法用实时的数据可能，因为变的太厉害，特别是livox

ego订阅的点云的坐标系是什么没关系，ego只读取值，最后发出来都是world坐标系发出来。上面自己没有补充任何Tf.

要不要试试订阅这个

这个压根没有发出来

这个也没有发出来

找了下fastlio并没有发出点云地图的话题，实时扫描的点云话题是有，但是比较稀疏又动得太快。

真正发出来有值的似乎就这三个话题

ego订阅/cloud_registered话题是有值的，按理用真实Livox是OK的吧！！！！可能他们用的这个livox是点云比较稀疏的 /cloud_registered话题确实把livox的原始点云转到了全局坐标系下，也就是结合了位姿变换的，这不就起到了我想要的作用么。ego可能也是出于这方面的考虑，现在的激光SLAM有发出转好的点云了！！！！！ R3LIVE 代码解析 - 知乎

是的，后面需要换种说法，ego订阅的是转到了全局坐标系下的激光雷达扫描点云。

这么看来，激光SLAM后的地图也靠不住了？

地图似乎最后存到pcl文件里面，并不是以话题的形式发出来的。确实如果是发话题，那太大了。

看来激光SLAM还是只能订阅实时的激光扫描点云，而且是全局坐标系下的实时扫描点云，激光SLAM来看也不会把地图以话题的形式发出来的。但是是可能发出转到全局坐标系下的实时扫描点云话题，这个可以用起来。

启动命令总共四个

roslaunch fast_lio mapping_avia.launch
rosbag play
roslaunch ego_planner rviz_temp.launch
roslaunch ego_planner run_in_exp_maxi_lidar_test.launch

换为真的livox mid40试试

激光雷达点云话题不对应，这里订阅的livox自己类型的，但是我写的话题是pointcoloud2类型的

得换个launch文件启动激光雷达

用livox_lidar_msg.launch启动后，发出的/livox/lidar话题就是这个类型的了，不是pointcloud2类型的了

从这看感觉都订阅到了呀

感觉可能跟配置文件不对应有关，faslio2里面对应的livox avia ，而我这是livox mid40，不是完全一样的，可能不能直接套。

找到问题所在了，用自己的Imu比如飞控的Imu的时候，这里需要改为true，看这里知道的 【自制】Livox_mid70外接维特imu运行fast_lio过程展示_哔哩哔哩_bilibili

如果没有改为true，fast lio跑不起来，而且终端一直显示这个

我也没有标定，直接这么跑起来看着还OK。

再让ego订阅faslio发出的odom和全局下的点云，启动ego，OK了，这个时候移动小车，点云不会也说跟着转了！

而且还不卡！

这是此时的节点图

这样的话是可以基于Livox在NX上跑fastlio+ego-planner了

这是此时的CPU，确实还好，而且这个时候我还nomachine远程了

这是此时的TF树

看下fastlio2输出的/Odometry，位姿没怎么飘，这个我的激光雷达和IMU的外参还没有怎么标定过。

移动快了似乎NX出现卡死的现象

总结一下基于livox跑FASTLIO2+ego-planner的操作

roslaunch livox_ros_driver livox_lidar_msg.launch   注意起这个launch文件，才能发出fastlio需要的点云话题类型
roslaunch mavros px4.launch
roslaunch fast_lio mapping_avia.launch    对应config文件需要修改
roslaunch ego_planner rviz.launch 
roslaunch ego_planner run_in_exp_310_with_livox.launch  注意订阅的话题，odom为/Odometry，激光点云话题为/cloud_registered

而且这样之后没有出现其实点就在障碍物内的情况了对吧！！！！！！ 在不是用livox自带IMU用比如飞控IMU的时候，配置文件里面time_sync_en这一项需要改为true，否则FASTLIO可能跑不起来。

看来不管是基于livox-loam还是FAST-LIO2去跑ego-planner都是可以的，订阅经过激光SLAM输出后的全局坐标系下的点云，这个对ego很关键，是全局坐标系下的点云，不是body坐标系下的点云。

此时的avia.yaml

common:
    lid_topic:  "/livox/lidar"
    imu_topic:  "/mavros/imu/data"
    time_sync_en: true         # ONLY turn on when external time synchronization is really not possible
​
preprocess:
    lidar_type: 1                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 
    scan_line: 6
    blind: 4
​
mapping:
    acc_cov: 0.1
    gyr_cov: 0.1
    b_acc_cov: 0.0001
    b_gyr_cov: 0.0001
    fov_degree:    90
    det_range:     450.0
    extrinsic_est_en:  false      # true: enable the online estimation of IMU-LiDAR extrinsic
    extrinsic_T: [ 0.04165, 0.02326, -0.0284 ]
    extrinsic_R: [ 1, 0, 0,
                   0, 1, 0,
                   0, 0, 1]
​
publish:
    path_en:  false
    scan_publish_en:  true       # false: close all the point cloud output
    dense_publish_en: true       # false: low down the points number in a global-frame point clouds scan.
    scan_bodyframe_pub_en: true  # true: output the point cloud scans in IMU-body-frame
​
pcd_save:
    pcd_save_en: true
    interval: -1                 # how many LiDAR frames saved in each pcd file; 
                                 # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

此时的run_in_exp_310_with_livox.launch

<launch>
  <!-- size of map, change the size inflate x, y, z according to your application -->
  <arg name="map_size_x" value="40.0"/>
  <arg name="map_size_y" value="40.0"/>
  <arg name="map_size_z" value=" 3.0"/>
​
  <!-- topic of your odometry such as VIO or LIO -->
  <arg name="odom_topic" value="/Odometry" />
  <!-- <arg name="odom_topic" value="/camera/odom/sample" /> --> 
​
  <!-- main algorithm params -->
  <include file="$(find ego_planner)/launch/advanced_param_droneyee.xml">
​
    <arg name="map_size_x_" value="$(arg map_size_x)"/>
    <arg name="map_size_y_" value="$(arg map_size_y)"/>
    <arg name="map_size_z_" value="$(arg map_size_z)"/>
    <arg name="odometry_topic" value="$(arg odom_topic)"/>
​
    <!-- camera pose: transform of camera frame in the world frame -->
    <!-- depth topic: depth image, 640x480 by default -->
    <!-- don't set cloud_topic if you already set these ones! -->
    <arg name="camera_pose_topic" value="/pcl_render_node/camera_pose"/>
    <arg name="depth_topic" value="/camera/depth/image_rect_raw"/>
​
    <!-- topic of point cloud measurement, such as from LIDAR  -->
    <!-- don't set camera pose and depth, if you already set this one! -->
    <!-- <arg name="cloud_topic" value="/livox/lidar"/> -->
    <arg name="cloud_topic" value="/cloud_registered"/>
​
    <!-- intrinsic params of the depth camera -->
    <arg name="cx" value="321.04638671875"/>
    <arg name="cy" value="243.44969177246094"/>
    <arg name="fx" value="387.229248046875"/>
    <arg name="fy" value="387.229248046875"/>
​
    <!-- maximum velocity and acceleration the drone will reach -->
    <arg name="max_vel" value="0.3" />
    <arg name="max_acc" value="0.5" />
​
    <!--always set to 1.5 times grater than sensing horizen-->
    <arg name="planning_horizon" value="7.5" /> 
​
    <!-- 1: use 2D Nav Goal to select goal  -->
    <!-- 2: use global waypoints below  -->
    <arg name="flight_type" value="1" />
    
    <!-- global waypoints -->
    <!-- It generates a piecewise min-snap traj passing all waypoints -->
    <arg name="point_num" value="5" />
​
    <arg name="point0_x" value="-15.0" />
    <arg name="point0_y" value="0.0" />
    <arg name="point0_z" value="1.0" />
​
    <arg name="point1_x" value="0.0" />
    <arg name="point1_y" value="15.0" />
    <arg name="point1_z" value="1.0" />
​
    <arg name="point2_x" value="15.0" />
    <arg name="point2_y" value="0.0" />
    <arg name="point2_z" value="1.0" />
​
    <arg name="point3_x" value="0.0" />
    <arg name="point3_y" value="-15.0" />
    <arg name="point3_z" value="1.0" />
​
    <arg name="point4_x" value="-15.0" />
    <arg name="point4_y" value="0.0" />
    <arg name="point4_z" value="1.0" />
    
  </include>
​
  <!-- trajectory server -->
  <node pkg="ego_planner" name="traj_server" type="traj_server" output="screen">
    <remap from="/position_cmd" to="planning/pos_cmd"/>
​
    <remap from="/odom_world" to="$(arg odom_topic)"/>
    <param name="traj_server/time_forward" value="1.0" type="double"/>
  </node>
​
  <node pkg="waypoint_generator" name="waypoint_generator" type="waypoint_generator" output="screen">
    <remap from="~odom" to="$(arg odom_topic)"/>        
    <remap from="~goal" to="/move_base_simple/goal"/>
    <remap from="~traj_start_trigger" to="/traj_start_trigger" />
    <param name="waypoint_type" value="manual-lonely-waypoint"/>    
  </node>
​
</launch>

launch文件调用的xml文件内容如下

<launch>
  <arg name="map_size_x_"/>
  <arg name="map_size_y_"/>
  <arg name="map_size_z_"/>

  <arg name="odometry_topic"/>
  <arg name="camera_pose_topic"/>
  <arg name="depth_topic"/>
  <arg name="cloud_topic"/>

  <arg name="cx"/>
  <arg name="cy"/>
  <arg name="fx"/>
  <arg name="fy"/>

  <arg name="max_vel"/>
  <arg name="max_acc"/>
  <arg name="planning_horizon"/>

  <arg name="point_num"/>
  <arg name="point0_x"/>
  <arg name="point0_y"/>
  <arg name="point0_z"/>
  <arg name="point1_x"/>
  <arg name="point1_y"/>
  <arg name="point1_z"/>
  <arg name="point2_x"/>
  <arg name="point2_y"/>
  <arg name="point2_z"/>
  <arg name="point3_x"/>
  <arg name="point3_y"/>
  <arg name="point3_z"/>
  <arg name="point4_x"/>
  <arg name="point4_y"/>
  <arg name="point4_z"/>

  <arg name="flight_type"/>

  <!-- main node -->
  <node pkg="ego_planner" name="ego_planner_node" type="ego_planner_node" output="screen">
    <remap from="/odom_world" to="$(arg odometry_topic)"/>
    <remap from="/grid_map/odom" to="$(arg odometry_topic)"/>
    <remap from="/grid_map/cloud" to="$(arg cloud_topic)"/>
    <remap from = "/grid_map/pose"   to = "$(arg camera_pose_topic)"/> 
    <remap from = "/grid_map/depth" to = "$(arg depth_topic)"/>

    <!-- planning fsm -->
    <param name="fsm/flight_type" value="$(arg flight_type)" type="int"/>
    <param name="fsm/thresh_replan" value="1.5" type="double"/>
    <param name="fsm/thresh_no_replan" value="2.0" type="double"/>
    <param name="fsm/planning_horizon" value="$(arg planning_horizon)" type="double"/> <!--always set to 1.5 times grater than sensing horizen-->
    <param name="fsm/planning_horizen_time" value="3" type="double"/>
    <param name="fsm/emergency_time_" value="1.0" type="double"/>

    <param name="fsm/waypoint_num" value="$(arg point_num)" type="int"/>
    <param name="fsm/waypoint0_x" value="$(arg point0_x)" type="double"/>
    <param name="fsm/waypoint0_y" value="$(arg point0_y)" type="double"/>
    <param name="fsm/waypoint0_z" value="$(arg point0_z)" type="double"/>
    <param name="fsm/waypoint1_x" value="$(arg point1_x)" type="double"/>
    <param name="fsm/waypoint1_y" value="$(arg point1_y)" type="double"/>
    <param name="fsm/waypoint1_z" value="$(arg point1_z)" type="double"/>
    <param name="fsm/waypoint2_x" value="$(arg point2_x)" type="double"/>
    <param name="fsm/waypoint2_y" value="$(arg point2_y)" type="double"/>
    <param name="fsm/waypoint2_z" value="$(arg point2_z)" type="double"/>
    <param name="fsm/waypoint3_x" value="$(arg point3_x)" type="double"/>
    <param name="fsm/waypoint3_y" value="$(arg point3_y)" type="double"/>
    <param name="fsm/waypoint3_z" value="$(arg point3_z)" type="double"/>
    <param name="fsm/waypoint4_x" value="$(arg point4_x)" type="double"/>
    <param name="fsm/waypoint4_y" value="$(arg point4_y)" type="double"/>
    <param name="fsm/waypoint4_z" value="$(arg point4_z)" type="double"/>

    <param name="grid_map/resolution"      value="0.1" /> 
    <param name="grid_map/map_size_x"   value="$(arg map_size_x_)" /> 
    <param name="grid_map/map_size_y"   value="$(arg map_size_y_)" /> 
    <param name="grid_map/map_size_z"   value="$(arg map_size_z_)" /> 
    <param name="grid_map/local_update_range_x"  value="5.5" /> 
    <param name="grid_map/local_update_range_y"  value="5.5" /> 
    <param name="grid_map/local_update_range_z"  value="4.5" /> 
    <param name="grid_map/obstacles_inflation"     value="0.099" /> 
    <param name="grid_map/local_map_margin" value="30"/>
    <param name="grid_map/ground_height"        value="-0.01"/>
    <!-- camera parameter -->
    <param name="grid_map/cx" value="$(arg cx)"/>
    <param name="grid_map/cy" value="$(arg cy)"/>
    <param name="grid_map/fx" value="$(arg fx)"/>
    <param name="grid_map/fy" value="$(arg fy)"/>
    <!-- depth filter -->
    <param name="grid_map/use_depth_filter" value="true"/>
    <param name="grid_map/depth_filter_tolerance" value="0.15"/>
    <param name="grid_map/depth_filter_maxdist"   value="5.0"/>
    <param name="grid_map/depth_filter_mindist"   value="0.2"/>
    <param name="grid_map/depth_filter_margin"    value="1"/>
    <param name="grid_map/k_depth_scaling_factor" value="1000.0"/>
    <param name="grid_map/skip_pixel" value="2"/>
    <!-- local fusion -->
    <param name="grid_map/p_hit"  value="0.65"/>
    <param name="grid_map/p_miss" value="0.35"/>
    <param name="grid_map/p_min"  value="0.12"/>
    <param name="grid_map/p_max"  value="0.90"/>
    <param name="grid_map/p_occ"  value="0.80"/>
    <param name="grid_map/min_ray_length" value="0.1"/>
    <param name="grid_map/max_ray_length" value="4.5"/>

    <param name="grid_map/virtual_ceil_height"   value="2.5"/>
    <param name="grid_map/visualization_truncate_height"   value="2.4"/>
    <param name="grid_map/show_occ_time"  value="false"/>
    <param name="grid_map/pose_type"     value="2"/>  
    <param name="grid_map/frame_id"      value="world"/>

  <!-- planner manager -->
    <param name="manager/max_vel" value="$(arg max_vel)" type="double"/>
    <param name="manager/max_acc" value="$(arg max_acc)" type="double"/>
    <param name="manager/max_jerk" value="4" type="double"/>
    <param name="manager/control_points_distance" value="0.4" type="double"/>
    <param name="manager/feasibility_tolerance" value="0.05" type="double"/>
    <param name="manager/planning_horizon" value="$(arg planning_horizon)" type="double"/>

  <!-- trajectory optimization -->
    <param name="optimization/lambda_smooth" value="1.0" type="double"/>
    <param name="optimization/lambda_collision" value="0.5" type="double"/>
    <param name="optimization/lambda_feasibility" value="0.1" type="double"/>
    <param name="optimization/lambda_fitness" value="1.0" type="double"/>
    <param name="optimization/dist0" value="0.5" type="double"/>
    <param name="optimization/max_vel" value="$(arg max_vel)" type="double"/>
    <param name="optimization/max_acc" value="$(arg max_acc)" type="double"/>

    <param name="bspline/limit_vel" value="$(arg max_vel)" type="double"/>
    <param name="bspline/limit_acc" value="$(arg max_acc)" type="double"/>
    <param name="bspline/limit_ratio" value="1.1" type="double"/>

  </node>

</launch>

我录了两个bag包，一个是只录了livox激光雷达话题，有一个是把基于livox跑FASTLIO2+EGO的所有话题都录下来了。

rosbag record -O livoxmid40_mavrosimu /livox/lidar /mavros/imu/data
rosbag record -O livoxmid40_mavrosimu_FASTLIO2_ego_all -a

录的bag包放到百度网盘里面去了

可以发现在激光扫描离得很近的物体的时候FASTLIO可能容易发散，可能是因为我没有标定激光雷达和IMU的原因吧。后面可以试试把config文件里面的那个IMU外参在线优化打开，看看是否会好些。

20220913 今天单纯跑FASTLIO2，这么近距离对着墙，没有发散。

单纯跑FAST-LIO2+nomachine远程时候的CPU使用率

刚开始前面没有规划出路径应该是位置z变为负的了！！！！

今天确实挪到这个程度FASTLIO2没有像昨天那样发散。

我把这改为true看看

改为外参自己估计后，近对着墙，效果也一般，甚至可能还不如。 而且可以看到FASTLIO刚一开始就把Z估计成负的好几厘米，可能是因为外参没有标定好。希望标定好是准确的，不然就连vins都抵不上了。

总体感受是把那个外参在线估计打开之后，位姿估计误差更大了，还不如不打开。 下面的位姿是我把无人车原地转了90度再转回来，位置偏差有几米，外参在线估计打开后的，没有打开的时候，这个偏差是比较小至少没有这么大的。

所以我还是改回了false

高度大于0的时候就规划出轨迹了

一般没有规划出路径确实就是astar找不出路径了。

这是FAST-LIO2+ego-planner都起起来时候的CPU，nomachine远程还开着

FAST-LIO2的输出位姿频率是10hz

最后给个启动的录屏视频截取