实现 mid360 仿真的接入
在虚拟环境中使用 Livox Mid360 驱动,需配置相应的 yaml 文件并编译相关依赖。
档详细记录了在 Ubuntu 18 环境下配置 ROS Melodic、PX4 SITL 及 XTDrone 仿真平台的过程,重点实现了 Livox Mid360 激光雷达在 Gazebo 中的集成与仿真。内容包括安装 Livox SDK 与驱动、解决 protoc 版本冲突及 Anaconda 环境干扰、修改 PX4 SDF 模型以挂载 Mid360 雷达、配置 Gazebo 插件路径、以及通过 MAVROS 和 XTDrone 进行无人机 Offboard 模式控制与 FAST-LIO 联合仿真。文中涵盖了从环境搭建、模型修改、编译调试到飞行控制脚本编写的完整技术链路,解决了常见的路径错误、插件加载失败及通信超时等问题。
在虚拟环境中使用 Livox Mid360 驱动,需配置相应的 yaml 文件并编译相关依赖。
git clone https://github.com/Livox-SDK/Livox-SDK2.git
cd ./Livox-SDK2/
mkdir build
cd build
cmake .. && make -j
sudo make install
# 在主目录下创建一个工作空间
mkdir -p catkin_ws/src
cd catkin_ws/src
git clone https://github.com/Livox-SDK/livox_ros_driver2.git
cd livox_ros_driver2
# 我是用的 ROS1,如果使用 ROS2 参考原仓库安装
./build.sh ROS1
# 激活全局路径
source ../../devel/setup.bash
# 验证,没报错的话基本上是没问题
roslaunch livox_ros_driver2 [launch file]
已安装好基础环境后,在此环境中安装 mid360 的仿真环境:
git clone https://github.com/qiurongcan/Mid360_imu_sim.git
cd ..
catkin_make
# 最好在~/.bashrc 文件中全局激活工作空间路径
source devel/setup.bash
# 验证
roslaunch livox_laser_simulation livox_simulation.launch
# 或者使用带有 IMU 版本的
roslaunch livox_laser_simulation mid360_IMU_platform.launch
编译时可能出现 protoc 版本不兼容或 math 库缺失错误。核心原因是 Ubuntu 18 对某些包支持度不佳,导致 math 和 protoc 版本冲突。建议更换兼容的库。
cd ~/catkin_ws/src
git clone https://github.com/Luchuanzhao/Livox_simulation_customMsg
mv Livox_simulation_customMsg livox_laser_simulation
# 修改激光雷达文件路径
cd ~/catkin_ws/src/livox_laser_simulation/src
# 将第 54 行的内容改为自己电脑实际的 csv 文件路径
# 例如:/home/user/catkin_ws/src/livox_laser_simulation/scan_mode/mid360.csv
# 将 101 行改为自己需要的点云格式:0->PointCloud;1/2->PointCloud2;3->CustomMsg
# 如果后续要使用 Faster-LIO,需使用 CustomMsg 还需注释掉 340、341 行的坐标变换代码
若提示 livox_ros_driver 找不到,需将 livox_laser_simulation 的 src、include 以及 CMakeLists.txt 中的 driver 引用全部改为 driver2,直到工作空间编译通过。
catkin_make
Anaconda 环境下的 protobuf 可能与系统库冲突。即使注释了 conda 环境变量,仍可能残留影响。
echo $PATH | grep conda
echo $LD_LIBRARY_PATH | grep conda
强制使用系统的 protoc(注意不要直接 export CMAKE_PREFIX_PATH=/usr,这会丢失 ROS 环境):
export Protobuf_INCLUDE_DIR=/usr/include
export Protobuf_LIBRARY=/usr/lib/x86_64-linux-gnu/libprotobuf.so
这是因为只加载了基础环境 /usr,找不到 ROS 等其他的依赖。
解决方案:
bash --noprofile --norcsource /opt/ros/melodic/setup.bashcd ~/proj/livox_wsrm -rf build develcatkin_make关键步骤:
conda deactivate,移除环境变量中可能存在的 conda 路径。CMAKE_PREFIX_PATH 包含 /opt/ros/melodic。Protobuf_INCLUDE_DIR 和 Protobuf_LIBRARY,不要动 CMAKE_PREFIX_PATH。build 和 devel 文件夹才能进行下一步。对于 Ubuntu 18,核心是解决 protoc 路径冲突,通过指定运行库解决。同时确保删除干净原来的编译 build、devel 再重装即可。
逻辑是通过 roslaunch px4 indoor1.launch 生成地图,该 launch 文件通过参数 sdf 选择无人机模型,无人机的 sdf 文件引入 Mid360 模型,最终搭配 mid360 的无人机同地图一起在 gazebo 中生成。
无人机模型和传感器模型都在 PX4 里,路径如下:
/home/user/PX4_Firmware/Tools/sitl_gazebo/models
复制一个无人机模型的文件夹(如 iris_3d_lidar),重命名为 iris_3d_lidar_360。
修改 iris_3d_lidar_360.sdf,在 sdf 中加入如下代码来引入 mid360,并屏蔽掉原始的雷达:
<!-- 添加雷达 -->
<include>
<uri>model://Mid360</uri>
<pose>0 0 0.05 0 0 0</pose>
</include>
<joint name="lidar_joint" type="fixed">
<child>Mid360::livox_base</child>
<parent>base_link</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<upper>0</upper>
<lower>0</lower>
</limit>
</axis>
</joint>
<!-- For Velodyne Lidar Payload (注释原始雷达) -->
<!-- <include> ... </include> -->
在 models 路径下添加文件夹 Mid360。将 Mid360 模型文件夹复制到 ~/PX4_Firmware/Tools/sitl_gazebo/models。
注意处理子文件夹冲突,确保 mesh 文件路径正确。修改 Mid360.sdf 中 <collision>、<visual> 标签所引用的文件路径,以及 <csv_file_name> 路径。
最后在 indoor1.launch 中将 vehicle 和 sdf 名称改对:
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_3d_lidar_360"/>
启动命令:
source /home/user/proj/xtdrone_ws/devel/setup.bash
source /home/user/proj/PX4_Firmware/Tools/setup_gazebo.bash
~/PX4_Firmware/
~/PX4_Firmware/build/px4_sitl_default
roslaunch px4 indoor1.launch
常见错误包括 Unknown model 或 URI 格式错误。
关键点 1:launch 文件中的无人机名称
PX4 只关心机型名(launch 里 <arg name="vehicle" value="..."/> 去掉前缀数字后的字符串)。Gazebo 只关心 SDF 文件名(launch 里 <arg name="sdf" value="..."/>)。
关键点 2:sdf 文件中的路径格式
URI 格式应为 model://model_name,而非绝对路径。错误示例:Invalid uri[model:/home/...]。应修正为 model://Mid360。
对于多旋翼飞行器,Offboard 控制起飞很容易,给一个大于 0.3m/s 的期望 z 向速度即可。
不断按 i 把期望 z 向速度加到 0.3m/s 以上,然后按 b 切到 offboard 模式,再按 t 解锁即可起飞,飞到合适的高度后,按 s 即可实现悬停。
若虚拟激光雷达不能输出点云和 imu 数据,大概率需要重新编译插件并确保路径正确。
export Protobuf_INCLUDE_DIR=/usr/include
export Protobuf_LIBRARY=/usr/lib/x86_64-linux-gnu/libprotobuf.so
catkin_make
注意每次编译前清理 build 目录。
在 ~/.bashrc 中添加必要的 source 路径,确保 Gazebo 能找到插件。
source /home/user/proj/xtdrone_ws/devel/setup.bash
source /home/user/proj/PX4_Firmware/Tools/setup_gazebo.bash
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:/home/user/proj/PX4_Firmware
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:/home/user/proj/PX4_Firmware/Tools/sitl_gazebo
插件路径问题:
Gazebo 查找插件 .so 文件的顺序:
GAZEBO_PLUGIN_PATHdevel/lib/解决方案:
方案 1:把插件复制到 xtdrone_ws 下。
方案 2:设置 GAZEBO_PLUGIN_PATH。
export GAZEBO_PLUGIN_PATH=$GAZEBO_PLUGIN_PATH:/home/user/proj/livox_ws/devel/lib
配置完成后,可通过可视化和键盘控制实现无人机的飞行定位建图。
mid360s.yaml:
common:
lid_topic: "/scan"
imu_topic: "/iris_0/imu_gazebo"
time_sync_en: false
time_offset_lidar_to_imu: 0.0
preprocess:
lidar_type: 1
scan_line: 4
blind: 0.5
mapping:
acc_cov: 0.1
gyr_cov: 0.1
extrinsic_T: [ -0.011, -0.02329, 0.04412 ]
extrinsic_R: [ 1, 0, 0, 0, 1, 0, 0, 0, 1]
publish:
scan_publish_en: true
dense_publish_en: true
conda deactivate
roslaunch px4 indoor1.launch
在另一终端运行通信脚本:
conda deactivate
cd /home/user/proj/XTDrone/communication/
python multirotor_communication.py iris 0
在另一终端运行控制脚本:
conda deactivate
cd /home/user/proj/XTDrone/control/keyboard
python multirotor_keyboard_control.py iris 1 vel
XTDrone 支持多旋翼、固定翼等机型。本教程介绍多旋翼配置。
PX4 支持多种飞行模式,XTDrone 主要利用 Offboard 模式。飞行器起飞前需解锁,着陆后需上锁。
推荐起飞流程:按 i 把向上速度加到 0.3 以上,再按 b 切 offboard 模式,最后按 t 解锁。
姿态控制指令发送给 mavros/setpoint_raw/attitude。该话题有两种控制方式:①角度 + 油门;②角速度 + 油门。
Gazebo 提供了无人机位姿姿态的真实值,可用于分析 SLAM 算法精度。
启动通信脚本获取真值:
cd ~/XTDrone/communication
python multirotor_communication.py iris 0
启动获取位姿真值的脚本:
cd ~/XTDrone/sensing/pose_ground_truth/
python get_local_pose.py iris 1
确定 gazebo、xtdrone 接收的控制信号形式。MAVROS 节点负责转换 MAVLink 消息与 ROS 消息。
常用话题:
/xtdrone/iris_0/cmd/xtdrone/iris_0/cmd_vel_flu键盘控制说明:
ego_planner 需要输入深度图 + 相机位姿或是点云。相关话题包括 /xtdrone/iris_0/planning/bspline 等。
定点飞行思路: 将 ego_planner 一条轨迹拆成多个轨迹,编写脚本分段执行任务。
旋转脚本示例:
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
def main():
rospy.init_node('command_publisher')
pub = rospy.Publisher('/xtdrone/iris_0/cmd_vel_flu', Twist, queue_size=10)
cmd_vel = Twist()
cmd_vel.angular.z = 1.57
rate = rospy.Rate(220)
count = 0
while not rospy.is_shutdown() and count < 240:
pub.publish(cmd_vel)
rate.sleep()
count += 1
if __name__ == '__main__':
try:
main()
except rospy.ROSInterruptException:
pass
执行任务脚本:
tmux new -d -s ego_planner
tmux send-keys -t ego_planner "roslaunch ego_planner single_uav.launch" ENTER
sleep 22
tmux send-keys -t ego_planner "C-c" ENTER
sleep 2
tmux send-keys -t ego_planner "python xuanzhuan.py" ENTER
注意脚本执行和仿真运行不是串联的,需预留足够睡眠时间以完成动作。

微信公众号「极客日志」,在微信中扫描左侧二维码关注。展示文案:极客日志 zeeklog
使用加密算法(如AES、TripleDES、Rabbit或RC4)加密和解密文本明文。 在线工具,加密/解密文本在线工具,online
生成新的随机RSA私钥和公钥pem证书。 在线工具,RSA密钥对生成器在线工具,online
基于 Mermaid.js 实时预览流程图、时序图等图表,支持源码编辑与即时渲染。 在线工具,Mermaid 预览与可视化编辑在线工具,online
将字符串编码和解码为其 Base64 格式表示形式即可。 在线工具,Base64 字符串编码/解码在线工具,online
将字符串、文件或图像转换为其 Base64 表示形式。 在线工具,Base64 文件转换器在线工具,online
将 Markdown(GFM)转为 HTML 片段,浏览器内 marked 解析;与 HTML转Markdown 互为补充。 在线工具,Markdown转HTML在线工具,online