本文主要是介绍在gazebo里搭建一个livox mid360 + 惯导仿真平台测试 FAST-LIO2,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!
在gazebo里搭建一个livox mid360 + 惯导仿真平台测试 FAST-LIO2
- 前言
- 立方体平台
- 加入 livox mid360 激光雷达
- 加入IMU模块
- 调整底盘大小 并设计调用接口
- 测试 Fast-Lio2
前言
livox mid360 在官网一直没有货,在gazebo里可以仿真该雷达形式的点云。
但是其只发布雷达的数据,没有imu数据,实际的雷达是可以发布既有雷达也有imu的数据的
运行 FAST-LIO2 也需要雷达和惯导的数据
本篇博客在gazebo中搭建了一个有livox mid360 和惯导的平台,并成功运行了FAST-LIO2
立方体平台
先做一个最简单的立方体平台,用来放livox mid360 和惯导
编写一个xacro文件,如下
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="example"><!-- Base Footprint --><link name="base_footprint" /><!-- Base Link --><joint name="footprint" type="fixed" ><parent link="base_footprint" /><child link="link_platform" /><origin xyz="0 0 0.05" rpy="0 0 0" /></joint><link name="link_platform" ><visual><geometry><box size="0.5 0.5 0.1" /></geometry></visual><collision><geometry><box size="0.5 0.5 0.1" /></geometry></collision><inertial><origin xyz="0 0 0"/><mass value="0.01"/><inertia ixx="0.001" ixy="0.0" ixz="0.0"iyy="0.001" iyz="0.0" izz="0.001" /></inertial></link></robot>
写一个launch文件来启动它,加载到gazebo和rivz中
<?xml version="1.0" ?>
<launch><arg name="paused" default="false"/><arg name="use_sim_time" default="true"/><arg name="gui" default="true"/><arg name="headless" default="false"/><arg name="debug" default="false"/><arg name="verbose" default="false"/><!-- Start gazebo and load the world --><include file="$(find gazebo_ros)/launch/empty_world.launch" ><arg name="paused" value="$(arg paused)"/><arg name="use_sim_time" value="$(arg use_sim_time)"/><arg name="gui" value="$(arg gui)"/><arg name="headless" value="$(arg headless)"/><arg name="debug" value="$(arg debug)"/><arg name="verbose" value="$(arg verbose)"/></include>
![在这里插入图片描述](https://img-blog.csdnimg.cn/1a7fdea939be47c8b0a1c6d7d064e1fa.png)![在这里插入图片描述](https://img-blog.csdnimg.cn/1a7fdea939be47c8b0a1c6d7d064e1fa.png)<!-- Spawn the platform --><param name="robot_description" command="$(find xacro)/xacro --inorder '$(find livox_laser_simulation)/urdf/mid360_IMU_platform.xacro' " /><node pkg="gazebo_ros" type="spawn_model" name="spawn_model" args="-urdf -param /robot_description -model example"/><node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"><param name="publish_frequency" type="double" value="30.0" /></node><!-- RViz --><arg name="rviz" default="true"/><node if="$(arg rviz)" pkg="rviz" type="rviz" name="$(anon rviz)" respawn="false" output="screen" args="-d $(find livox_laser_simulation)/rviz/mid360_IMU_platform.rviz" /></launch>
启动
roslaunch livox_laser_simulation mid360_IMU_platform.launch
gazebo 和rviz 中会出现下面的立方体,具体的尺寸和颜色可以最后再根据雷达和惯导再调整
加入 livox mid360 激光雷达
下一步向平台上加入 livox mid360 激光雷达, 放在平台的中心。
向 livox_laser_simulation/urdf/mid360_IMU_platform.xacro 文件中加入下面代码
<joint name="lidar_platform" type="fixed" ><parent link="link_platform" /><child link="livox_base" /><origin xyz="0 0 0.08" rpy="0 0 0" /></joint><xacro:include filename="$(find livox_laser_simulation)/urdf/livox_mid360.xacro"/><xacro:Livox_Mid360 name="livox"/>
再次启动launch文件即可,看到平台中央多了livox mid360 雷达
确认雷达输出点云正常,可向周围加入几个物体测试
点云正常
加入IMU模块
向平台中加入IMU模块,放在雷达的前方一点
向 livox_laser_simulation/urdf/mid360_IMU_platform.xacro 文件中加入下面代码
<!--imu --><link name="imu_base_link"><visual><origin xyz="0 0 0" rpy="0 0 0" /><geometry ><box size="0.03 0.03 0.03" /></geometry></visual><collision><origin xyz="0 0 0" rpy="0 0 0" /><geometry ><box size="0.03 0.03 0.03" /></geometry></collision> <inertial><mass value="0.001"/><inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/></inertial> </link><gazebo reference="imu_base_link"><material>Gazebo/Green</material><turnGravityOff>true</turnGravityOff></gazebo><joint name="imu_platform_joint" type="fixed"><parent link="link_platform"/><child link="imu_base_link"/><origin xyz="0.05 0 0.065" rpy="0 0 0" /><axis xyz="0 0 1" /></joint><gazebo reference="imu_base_link"><gravity>true</gravity><sensor name="imu_sensor" type="imu"><always_on>true</always_on><update_rate>200</update_rate><visualize>true</visualize><topic>/livox/imu</topic><plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin"><topicName>/livox/imu</topicName> <bodyName>imu_base_link</bodyName><updateRateHZ>200.0</updateRateHZ> <gaussianNoise>0.00329</gaussianNoise> <xyzOffset>0 0 0</xyzOffset> <rpyOffset>0 0 0</rpyOffset><frameName>imu_base_link</frameName> </plugin><pose>0 0 0 0 0 0</pose></sensor></gazebo>
再次启动launch文件即可,看到平台中 的livox mid360 雷达 前多了一个绿色的小方块,它即为刚建立惯导模型
通过rostopic 查看 惯导数据是否正常
数据正常
调整底盘大小 并设计调用接口
下一步需要调整底盘平台的大小,能够放下雷达和惯导即可
将底盘尺寸改为如下
<box size="0.15 0.1 0.1" />
回来平台的前方挂个双目也不错
带有雷达和惯导的平台搭建完了
但是这个平台无法运动,也就完成不了建图,如果运行需要将平台挂载到小车或者无人机上。
所以需要对这个平台设计好接口,方便其它移动平台挂载。
修改 livox_laser_simulation/urdf/mid360_IMU_platform.xacro 这个文件
最后的文件如下:
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="example"><!-- Base Footprint --><!-- <link name="base_footprint" /> --><xacro:macro name="LivoxMid360_IMU_Plantform" params="name:=mid360_imu_plantform parent_link_name:=base_link x:=0.0 y:=0.0 z:=0.05 r:=0.0 p:=0.0 yaw:=0.0"><joint name="${name}_joint" type="fixed" ><parent link="${parent_link_name}" /><child link="link_platform" /><origin xyz="${x} ${y} ${z}" rpy="${r} ${p} ${yaw}" /></joint><!-- link_platform --><link name="link_platform" ><visual><geometry><box size="0.15 0.1 0.1" /></geometry></visual><collision><geometry><box size="0.15 0.1 0.1" /></geometry></collision><inertial><origin xyz="0 0 0"/><mass value="0.001"/><inertia ixx="0.001" ixy="0.0" ixz="0.0"iyy="0.001" iyz="0.0" izz="0.001" /></inertial></link><gazebo reference="link_platform"><turnGravityOff>false</turnGravityOff></gazebo><joint name="lidar_platform_joint" type="fixed" ><parent link="link_platform" /><child link="livox_base" /><origin xyz="0 0 0.08" rpy="0 0 0" /></joint><!--lidar --><xacro:include filename="$(find livox_laser_simulation)/urdf/livox_mid360.xacro"/><xacro:Livox_Mid360 name="livox"/><!--imu --><link name="imu_base_link"><visual><origin xyz="0 0 0" rpy="0 0 0" /><geometry ><box size="0.03 0.03 0.03" /></geometry></visual><collision><origin xyz="0 0 0" rpy="0 0 0" /><geometry ><box size="0.03 0.03 0.03" /></geometry></collision> <inertial><mass value="0.001"/><inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/></inertial></link><gazebo reference="imu_base_link"><material>Gazebo/Green</material><turnGravityOff>false</turnGravityOff></gazebo><joint name="imu_platform_joint" type="fixed"><parent link="link_platform"/><child link="imu_base_link"/><origin xyz="0.05 0 0.065" rpy="0 0 0" /><axis xyz="0 0 1" /></joint><gazebo reference="imu_base_link"><gravity>true</gravity><sensor name="imu_sensor" type="imu"><always_on>true</always_on><update_rate>200</update_rate><visualize>true</visualize><topic>/livox/imu</topic><plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin"><topicName>/livox/imu</topicName> <bodyName>imu_base_link</bodyName><updateRateHZ>200.0</updateRateHZ> <gaussianNoise>0.00329</gaussianNoise> <xyzOffset>0 0 0</xyzOffset> <rpyOffset>0 0 0</rpyOffset><frameName>imu_base_link</frameName> </plugin><pose>0 0 0 0 0 0</pose></sensor></gazebo></xacro:macro></robot>
将其挂载到飞机上,代码如下:的几款雷达进行了仿真测试。
<xacro:include filename="$(find livox_laser_simulation)/urdf/mid360_IMU_platform.xacro"/><xacro:LivoxMid360_IMU_Plantform name ="mid360_imu_plantform" parent_link_name="${namespace}/base_link" x="0" y="0" z="0.08" r="0" p="0" yaw="0" />
最终效果如下:
测试 Fast-Lio2
用这个平台跑Fast-Lio2 还需要有一步,就是修改雷达的仿真程序,使其发送Livox的Custcom格式的点云,这个方法会放到其它博客中
调通后,用其跑Fast-Lio2 效果如下:
这篇关于在gazebo里搭建一个livox mid360 + 惯导仿真平台测试 FAST-LIO2的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!