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

Livox_Mid360+IMU仿真搭建_mid360自带惯导吗

作者：小惠珠哦 | 2024-08-14 23:10:39

赞

踩

mid360自带惯导吗

前言

本文是对在gazebo里搭建一个livox mid360 + 惯导仿真平台测试 FAST-LIO2的实现，此博文中存在部分需要修改代码的地方，因此在本文中做出更详细的说明。

资源包安装

注：由于livox点云的格式是CustomMsg，而rviz中主要使用PointCloud和PointCloud2，转换不太方便，因此不使用官方提供的软件包，使用他人修改后的软件包。但此软件包开发时间较早，缺少mid360相关的文件，在文末提供了mid360相关文件的下载链接，也可直接从官方链接下载

cd ~/catkin_ws/src
git clone https://github.com/Luchuanzhao/Livox_simulation_customMsg.git
mv Livox_simulation_customMsg livox_laser_simulation

  • 1
  • 2
  • 3

还需安装livox_ros_driver

cd ~/catkin_ws/src
git clone https://github.com/Livox-SDK/livox_ros_driver.git

  • 1
  • 2

再修改csv文件路径/点云格式

mid360.csv文件可从提供的资源包中得到

#将第54行改为实际的csv文件路径
cd ~/catkin_ws/src/livox_laser_simulation/src
code livox_points_plugin.cpp
#例如（/home/byy/catkin_ws/src/livox_laser_simulation/scan_mode/mid360.csv）
#将101行改为自己所需要的点云格式：0->PointCloud；1/2->PointCloud2；3->CustomMsg
#例如：publishPointCloudType = 1;

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6

还需注释掉340、341行的坐标变换代码，要不然rviz中得不到图像

//tfBroadcaster->sendTransform(
//   tf::StampedTransform(tf, ros::Time::now(), raySensor->ParentName(), raySensor->Name()));

  • 1
  • 2

模型搭建

cd ~/catkin_ws/src/livox_laser_simulation/urdf
code mid360_IMU_platform.xacro

  • 1
  • 2

使用原博文提供的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>
<xacro:LivoxMid360_IMU_Plantform/>
</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
  • 109

在meshes文件夹中引入提供的mid360.dae、mid360_rules.dae文件

还需在urdf文件夹中编写livox_mid360.xacro文件，并修改部分代码，具体如下。

<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">

  <xacro:property name="M_PI" value="3.14159"/> 
  <xacro:property name="laser_min_range" value="0.1"/>
  <xacro:property name="laser_max_range" value="200.0"/>
  <xacro:property name="ros_topic" value="scan"/>
  <xacro:property name="samples" value="24000"/>
  <xacro:property name="downsample" value="1"/>
  
  <xacro:macro name="null_inertial">
    <inertial>
      <mass value="0.1"/>
    <inertia ixx="0.01" ixy="0" ixz="0"
          iyy="0.01" iyz="0"
          izz="0.01"/>
    </inertial>
  </xacro:macro>
  <xacro:macro name="Livox_Mid_gazebo_sensor" params="visualize:=True update_rate:=10 resolution:=0.002 noise_mean:=0.0 noise_stddev:=0.01 name:=livox">
    <gazebo reference="${name}">
      <sensor type="ray" name="laser_${name}">
        <pose>0 0 0 0 0 0</pose>
        <visualize>${visualize}</visualize>
        <update_rate>${update_rate}</update_rate>
        <!-- This ray plgin is only for visualization. -->
        <plugin name="gazebo_ros_laser_controller" filename="liblivox_laser_simulation.so">
			 <ray>
			  <scan>
				  <horizontal>
				    <samples>100</samples>
				    <resolution>1</resolution>
				    <min_angle>0</min_angle>
            			    <max_angle>${2*M_PI}</max_angle>
				  </horizontal>
				  <vertical>
				    <samples>360</samples>
				    <resolution>1</resolution>
				    <min_angle>${-3.22/180*M_PI}</min_angle>
           			    <max_angle>${56.22/180*M_PI}</max_angle>
				  </vertical>
			  </scan>
			  <range>
				  <min>0.1</min>
				  <max>30</max>
				  <resolution>0.02</resolution>
			  </range>
			  <noise>
				  <type>gaussian</type>
				  <mean>0.0</mean>
				  <stddev>0.0</stddev>
			  </noise>
			  </ray>
          <visualize>${visualize}</visualize>
		  <samples>${samples}</samples>
		  <downsample>${downsample}</downsample>
		  <csv_file_name>package://livox_laser_simulation/scan_mode/mid360.csv</csv_file_name>
		  <ros_topic>${ros_topic}</ros_topic>
        </plugin>
      </sensor>
    </gazebo>
  </xacro:macro>


  <xacro:macro name="Livox_Mid40" params="visualize:=True name:=livox">
    <link name="${name}_base">
      <xacro:null_inertial/>
      <visual> 
        <origin xyz="0.00 0 0.0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://livox_laser_simulation/meshes/mid360.dae">
          </mesh>
        </geometry>
      </visual>

      <collision>
		  <origin xyz="0 0 0" rpy="0 0 0" /> <geometry> <mesh 
		  filename="package://livox_laser_simulation/meshes/mid360_rules.dae"> 
		  </mesh> </geometry>
      </collision>
    </link>
    <link name="${name}">
      <xacro:null_inertial/>
    </link>

    <joint name="${name}_to_${name}_base_joint" type="fixed">
      <parent link="${name}_base"/>
      <child link="${name}"/>
      <origin rpy="0 0 0" xyz="-0.012 0.0 0.047"/>
    </joint>
    <xacro:Livox_Mid_gazebo_sensor name="${name}" visualize="${visualize}"/>
  </xacro:macro>
    <xacro:Livox_Mid40 name="livox"/>
  <!--xacro:include filename="$(find livox_laser_simulation)/urdf/standardrobots_oasis300.xacro"/>
  <xacro:link_oasis name="oasis"/-->
</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

编写launch文件

cd ~/catkin_ws/src/livox_laser_simulation/launch
code mid360_IMU_platform.launch

  • 1
  • 2
<?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>

  <!-- 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 name="rviz" pkg="rviz" type="rviz" args="-d $(find livox_laser_simulation)/rviz/livox_simulation.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

编译运行

cd ~/catkin_ws
catkin_make

  • 1
  • 2

注：有时修改完文件立刻编译会报错:

fatal error: livox_ros_driver/CustomMsg.h:No such file and directory

  • 1

主要原因应该是多线程编译导致的，将终端关闭，重新编译即可
参考：https://blog.csdn.net/qq_59475883/article/details/127758942

roslaunch livox_laser_simulation mid360_IMU_platform.launch

  • 1

在这里插入图片描述

资源下载

链接: https://pan.baidu.com/s/17x61LmNLG7_-S4OY7IuE7Q?pwd=1234 提取码: 1234

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

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

  

闽ICP备14008679号