LIVOX-mid360+fastlio+ego--planner实际结合（无人机实际定位、建图、导航、避障）

首先大致介绍一下机载电脑用的香橙派5max（装的ubuntu20.04及ROS1），激光雷达是mid360，最后在rviz实时显示香橙派还“吃得消”。

目录

一、机载电脑运行雷达驱动，正常读取到雷达与IMU数据

二、启动slam（fastlio），发布ego需要的话题

三、ego运行

一、机载电脑运行雷达驱动，正常读取到雷达与IMU数据

这部分工作没做的可以看我这篇，非常详细，另外下面讲下注意事项

https://blog.csdn.net/2402_82745259/article/details/142923178?spm=1001.2014.3001.5502

注意到最后这步时，别运行roslaunch livox_ros_driver2 rviz_MID360.launch而是改成

roslaunch livox_ros_driver2 msg_MID360.launch

然后打开一个终端查看雷达与IMU消息类型（为后面做准备）

输入

 rostopic type /livox/imu

出现

然后输入

rostopic type /livox/lidar

出现

以上两次出现跟我一样的消息类型即可，否则进入/livox_ros_driver2/launch_ROS1下，更改里面的msg_MID360.launch中的xfer_format对应的default，改成1。如下

然后再次运行launch文件，打印雷达与IMU话题的类型应该就跟我上面的一样了。

二、启动slam（fastlio），发布ego需要的话题

这部分工作我之前也详细讲过，可以看下我这篇，另外下面讲下注意事项

https://blog.csdn.net/2402_82745259/article/details/144032036?spm=1001.2014.3001.5502

注意最后一步在终端运行roslaunch fast_lio mapping_mid360.launch之前，将mapping_mid360.launch文件里面的rviz显示注释掉，没有必要显示，比较耗费资源。如下

<launch>
<!-- Launch file for Livox AVIA LiDAR -->

        <arg name="rviz" default="true" />

        <rosparam command="load" file="$(find sfast_lio)/config/mid360.yaml" />

        <param name="feature_extract_enable" type="bool" value="0"/>
        <!-- 100HZ的bag  point_filter_num建议设置为1;   10HZ的bag建议设置为2或3 -->
        <param name="point_filter_num" type="int" value="3"/>
        <param name="max_iteration" type="int" value="3" />
        <param name="filter_size_surf" type="double" value="0.5" />
        <param name="filter_size_map" type="double" value="0.5" />
        <param name="cube_side_length" type="double" value="1000" />
    <node pkg="sfast_lio" type="fastlio_mapping" name="laserMapping" output="screen" />

        <!--<group if="$(arg rviz)">
        <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find sfast_lio)/rviz_cfg/loam_livox.rviz" />
        </group>-->

</launch>

然后编译再运行

roslaunch fast_lio mapping_mid360.launch

然后打开一个新的终端查看话题，输入

rostopic list

出现

可以看下/Odometry与/cloud_registered话题消息是否存在，输入

rostopic echo /Odometry
rostopic echo /cloud_registered

正常出现即可。再看下位姿有没有飘（雷达一般很稳定）。

三、ego运行

先下载源码

https://github.com/ZJU-FAST-Lab/Fast-Drone-250

然后更改​​​​​​single_run_in_sim.launch，如下

<launch>
    <!-- number of moving objects -->
    <arg name="obj_num" value="10" />
    <arg name="drone_id" value="0"/>

    <arg name="map_size_x" value="50.0"/>
    <arg name="map_size_y" value="25.0"/>
    <arg name="map_size_z" value=" 2.0"/>
    <arg name="odom_topic" value="/Odometry" />
    
    <!-- map -->
    <node pkg ="map_generator" name ="random_forest" type ="random_forest" output = "screen">
        <param name="map/x_size" value="26" />
        <param name="map/y_size" value="20" />
        <param name="map/z_size" value="3" />
        <param name="map/resolution" value="0.1"/>
        <param name="ObstacleShape/seed" value="1"/>
        <param name="map/obs_num" value="100"/>
        <param name="ObstacleShape/lower_rad" value="0.5"/>
        <param name="ObstacleShape/upper_rad" value="0.7"/>
        <param name="ObstacleShape/lower_hei" value="0.0"/>
        <param name="ObstacleShape/upper_hei" value="3.0"/>
        <param name="map/circle_num" value="100"/>
        <param name="ObstacleShape/radius_l" value="0.7"/>
        <param name="ObstacleShape/radius_h" value="0.5"/>
        <param name="ObstacleShape/z_l" value="0.7"/>
        <param name="ObstacleShape/z_h" value="0.8"/>
        <param name="ObstacleShape/theta" value="0.5"/>
        <param name="pub_rate" value="1.0"/>
        <param name="min_distance" value="0.8"/>
    </node>

    <!-- main algorithm params -->
    <include file="$(find ego_planner)/launch/advanced_param.xml">
        <arg name="drone_id" value="$(arg drone_id)"/>
        <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)"/>
        <arg name="obj_num_set" value="$(arg obj_num)" />
        <!-- 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="pcl_render_node/depth"/>
        <!-- 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="/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="2.0" />
        <arg name="max_acc" value="6.0" />
        <!--always set to 1.5 times grater than sensing horizen-->
        <arg name="planning_horizon" value="7.5" />
        <arg name="use_distinctive_trajs" value="true" />
        <!-- 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="4" />
        <arg name="point0_x" value="15" />
        <arg name="point0_y" value="0" />
        <arg name="point0_z" value="1" />
        <arg name="point1_x" value="-15.0" />
        <arg name="point1_y" value="0.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="-15.0" />
        <arg name="point3_y" value="0.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="drone_$(arg drone_id)_traj_server" type="traj_server" output="screen">
        <remap from="position_cmd" to="drone_$(arg drone_id)_planning/pos_cmd"/>
        <remap from="~planning/bspline" to="drone_$(arg drone_id)_planning/bspline"/>
        <param name="traj_server/time_forward" value="1.0" type="double"/>
    </node>

    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find ego_planner)/launch/sim.rviz" required="true" />

    <!-- use simulator -->
    <include file="$(find ego_planner)/launch/simulator.xml">
        <arg name="drone_id" value="$(arg drone_id)"/>
        <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="init_x_" value="-15"/>
        <arg name="init_y_" value="0"/>
        <arg name="init_z_" value="0.1"/>
        <arg name="odometry_topic" value="$(arg odom_topic)" />
    </include>
</launch>

然后编译再运行

catkin_make
source devel/setup.bash
roslaunch ego_planner single_run_in_sim.launch

出现如下

然后rviz显示

roslaunch ego_planner rviz.launch

出现

效果可以