devbox
盐析白兔
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

在gazebo里搭建一个livox mid360 + 惯导仿真平台测试 FAST-LIO2_mid360 gazebo

作者：盐析白兔 | 2024-05-02 09:46:41

赞

踩

mid360 gazebo

在gazebo里搭建一个livox mid360 + 惯导仿真平台测试 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>

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34

写一个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>

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33

启动

roslaunch livox_laser_simulation mid360_IMU_platform.launch

  • 1

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"/>

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8

再次启动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>

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51

再次启动launch文件即可，看到平台中 的livox mid360 雷达 前多了一个绿色的小方块，它即为刚建立惯导模型
在这里插入图片描述
通过rostopic 查看 惯导数据是否正常
在这里插入图片描述
数据正常

调整底盘大小 并设计调用接口

下一步需要调整底盘平台的大小，能够放下雷达和惯导即可

将底盘尺寸改为如下

<box size="0.15 0.1 0.1" />

  • 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>

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104
  • 105
  • 106
  • 107
  • 108

将其挂载到飞机上，代码如下：的几款雷达进行了仿真测试。

  <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" /> 

  • 1
  • 2

最终效果如下：
在这里插入图片描述

测试 Fast-Lio2

用这个平台跑Fast-Lio2 还需要有一步，就是修改雷达的仿真程序，使其发送Livox的Custcom格式的点云，这个方法会放到其它博客中

调通后，用其跑Fast-Lio2 效果如下：

在这里插入图片描述

在这里插入图片描述

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/盐析白兔/article/detail/523411
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号