无人机避障——Mid360+Fast-lio感知建图+Ego-planner运动规划（胎教级教程）

电脑配置：Xavier-nx、ubuntu 18.04、ros melodic

激光雷达：Livox_Mid-360

结果展示：左边Mid360+Fast-lio感知建图，右边Ego-planner运动规划

1、读取雷达数据并显示

无人机避障——感知篇（采用Livox-Mid360激光雷达获取点云数据显示）-CSDN博客

看看雷达数据话题imu以及lidar两个话题

2、读取雷达数据并复现fast-lio

无人机避障——感知篇（采用Mid360复现Fast-lio）-CSDN博客

启动fast-lio，确保话题有输出

由于此处不需要建图，因此不打开rviz，launch文件如下修改：

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

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

	<rosparam command="load" file="$(find fast_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" />
	<param name="runtime_pos_log_enable" type="bool" value="0" />
    <node pkg="fast_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 fast_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

/Odometry结果：

/cloud_registered结果：

3、下载ego-planner源码并编译运行

下载源码：

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

[注意]：根据不同的报错下载相应的包，因为这个包会携带实际飞行的Mavros包，以及视觉包，进入到上面的github界面以后，可以把第七章的内容全部安装一下，不然catkin_make的时候会报错，当然也可以直接编译，等报哪个错的时候进行解决就可以了。

Opencv报错：

其他的报错都还好，碰到了比较麻烦的opencv路径版本等报错，解决时间比较长。总结的报错如下：

报错1：

CMake Error at /opt/ros/melodic/share/cv_bridge/cmake/cv_bridgeConfig.cmake:113

解决：

CMake Error at /opt/ros/melodic/share/cv_bridge/cmake/cv_bridgeConfig.cmake:113-CSDN博客

报错2：

nvidia@Xavier-NX:~/Fast-Drone-250$ locate opencv2/core/core.hpp /home/nvidia/opencv/modules/core/include/opencv2/core/core.hpp /usr/include/opencv4/opencv2/core/core.hpp /usr/local/opencv346/include/opencv2/core/core.hpp

解决：

1、通过vscode的全局搜索功能，将find_package(OpenCV 4 REQUIRED)和find_package(OpenCV 3 REQUIRED)全部替换成find_package(OpenCV REQUIRED)。

2、如果 OpenCV 安装在非标准路径，可以通过以下命令检查 opencv2/core/core.hpp 的位置：

locate opencv2/core/core.hpp

3、如果 OpenCV 被安装在非标准路径也就是上面找到的路径，可以通过设置环境变量来让编译器找到头文件。你可以在终端中运行以下命令：

[注意]：/usr/local/opencv346/include这个是我用第二步locate到的路径，如果没有locate出来的话应该是没有安装opencv，建议安装opencv3.

export CPATH=$CPATH:/usr/local/opencv346/include

或者，在 .bashrc 文件中添加以下行：

export CPATH=$CPATH:/usr/local/opencv346/include

然后重新加载 .bashrc 文件：

source ~/.bashrc

重新编译：

编译成功！！！

只启动运动规划端仿真-ego-planner

nvidia@Xavier-NX:~/Fast-Drone-250$ source devel/setup.bash
nvidia@Xavier-NX:~/Fast-Drone-250$ roslaunch ego_planner single_run_in_sim.launch

视频如下：

Ego-planner仿真-CSDN直播

Ego-planner仿真

启动mid360建图fast-lio到ego-planner运动规划仿真：

然后为了在仿真中测试下从mid360经过fast-lio得到的建图和ego-planner进行运动规划，在single_run_in_exp.launch文件中进行odom_topic和cloud_topic两个话题的更改为mid360中的/Odometry与/cloud_registered话题如下：

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

    <arg name="map_size_x" value="100"/>
    <arg name="map_size_y" value="50"/>
    <arg name="map_size_z" value="3.0"/>
    <arg name="odom_topic" value="/Odometry"/>
    
    <!-- main algorithm params -->
    <include file="$(find ego_planner)/launch/advanced_param_exp.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="nouse1"/>
        <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="/cloud_registered"/>
        <!-- intrinsic params of the depth camera -->
        <arg name="cx" value="323.3316345214844"/>
        <arg name="cy" value="234.95498657226562"/>
        <arg name="fx" value="384.39654541015625"/>
        <arg name="fy" value="384.39654541015625"/>
        <!-- maximum velocity and acceleration the drone will reach -->
        <arg name="max_vel" value="0.5" />
        <arg name="max_acc" value="6.0" />
        <!--always set to 1.5 times grater than sensing horizen-->
        <arg name="planning_horizon" value="6" />
        <arg name="use_distinctive_trajs" value="false" />
        <!-- 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="1" />
        <arg name="point0_x" value="15" />
        <arg name="point0_y" value="0" />
        <arg name="point0_z" value="1" />
        <arg name="point1_x" value="0.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="0.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="/setpoints_cmd"/> -->
        <remap from="~planning/bspline" to="drone_$(arg drone_id)_planning/bspline"/>
        <param name="traj_server/time_forward" value="1.0" type="double"/>
    </node>
</launch>

启动launch文件：

nvidia@Xavier-NX:~/Fast-Drone-250$ roslaunch ego_planner single_run_in_exp.launch
nvidia@Xavier-NX:~/Fast-Drone-250$ roslaunch ego_planner rviz.launch

视频如下：

Mid360+Fastlio-SLAM+Egoplanner-CSDN直播

Mid360+Fastlio-SLAM+Egoplanner

后续进行实际飞行准备测试！！！

4、参考资料：

基于fast-lio2来跑下ego-planner（最后基于真实的livox mid 40静态下跑了） 20220913_fast-lio ego-planner-CSDN博客

LIVOX-mid360+fastlio+ego--planner实际结合（无人机实际定位、建图、导航、避障）_slam无人机mid360-CSDN博客自己部署FAST LIO2操作记录 20220912_fastlio2安装-CSDN博客

自己基于livox mid40跑FAST-LIO2 20220921_livoxmid40 fast-lio-CSDN博客