基于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
最后给个启动的录屏视频截取
NX上静态跑通基于livox mid40的FASTLIO2+ego-planner 20220912
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
