gazebo添加livox传感器进行仿真

一、说明

因为课题原因,需要用到livox传感器,但是总拿着它去实验太费劲儿,所以想要在ros的gazebo环境里面进行仿真实验。在网上找了一圈,都没有详细的教程,所以自己摸索了一会儿,大致上解决了这个点。过程不难,在这里记录一下,一方面希望可以帮到其他有需要的人,另一方面也是给自己做个记录,防止忘了。

二、仿真环境

1、ubuntu20.04
2、ros是noetic
3、机器人小车用的是husky

三、配置过程

1、安装livox的SDK

下载链接:https://github.com/Livox-SDK/Livox-SDK
安装过程在代码包里面交代得比较清楚了,这里也贴一下。

git clone https://github.com/Livox-SDK/Livox-SDK.git
cd Livox-SDK
cd build && cmake ..
make
sudo make install

整个环境的兼容性比较高,编译没有出现什么错误。

2、安装livox的ROS驱动

下载链接:https://github.com/Livox-SDK/livox_ros_driver
安装步骤与一般的ros包无差别,下载后catkin_make即可。

git clone https://github.com/Livox-SDK/livox_ros_driver.git ws_livox/src
cd ws_livox
catkin_make

3、安装livox的仿真环境包

下载链接:https://github.com/Livox-SDK/livox_laser_simulation
注意:这个安装包,针对的是Ubuntu 18.04以及之前的版本,用Ubuntu 20.04编译的话,需要把它的CMakeLists.txt里面的编译器版本改为17,即:将下面这句话里的11改为17:

add_compile_options(-std=c++11)

否则会出现如下报错:

。。。error:anyis not a member of ‘std’。。。

详细可以参考,代码仓库的Issues#4

4、安装husky小车的仿真包

下载链接:https://github.com/husky/husky
安装过程不再赘述,期间可能会报一些依赖包缺失,缺啥安装啥就行。

5、把livox放到小车上(敲黑板!!!重点哈~

1、思考,两个独立的包,小车仿真可以开启,livox的仿真也可以开启,没道理说它俩放不到一起去。既然每个包,都是urdf文件,那么,是不是可以说,只要把这俩urdf文件的关键信息结合到一起,就能完成了呢?!试试看。
2、然后想起来,以前也给小车添加过VLP-16激光雷达,这俩或许可以相通。添加VLP-16的教程,大家可以参考以下两个链接:
教程一:ROS仿真笔记之——gazebo配置velodyne(引用CSDN博主:gwpscut)
教程二:[gazebo仿真]给模型添加多线激光雷达(引用CSDN博主:Travis.X)
3、有了上面的基础,就可以开整了,我们可以观察到,其实添加一个传感器,主要就是两部分:
**第一部分:**在你想要添加到的小车的URDF文件里,创建一个实体,用来代表这个传感器,并且将它固定到小车上,比如,我们现在想要把livox添加到husky的小车上,那么第一步,就是打开husky功能包里面的/husky/husky_description/urdf/husky.urdf.xacro这个文件,然后在那一堆link标签后面,添加:

<link name="laser_livox">
	<collision>
	  <origin xyz="0 0 0" rpy="0 0 0"/>  
	  <geometry>
		<box size="0.1 0.1 0.1"/>
	  </geometry>
	</collision>	
	<visual>
	  <origin xyz="0 0 0" rpy="0 0 0"/>
	  <geometry>
		<box size="0.1 0.1 0.1"/>
	  </geometry>
	</visual>
  </link>

这是创建了一个边长为0.1m的正方体,用来表示livox这个固态雷达,你也可以把它设置成你想要的形状,无关紧要,然后紧接着上面那段,添加一个关节,把它跟小车的坐标系连接起来:

<joint name="laser_livox_joint" type="fixed" >
    <origin xyz="0 0 0.5" rpy="0 0 0" />
    <parent link="base_link" />
    <child link="laser_livox"/>
</joint>

上面的完成之后,就可以进入到第二部分了。
第二部分:为上面的小方块注入灵魂,为它赋予固态雷达的使命:

<xacro:property name="horizontal_fov" value="70.4"/>
<xacro:property name="vertical_fov" value="70.4"/>
  <gazebo reference="laser_livox">
      <sensor type="ray" name="laser_livox">
        <pose>0 0 0 0 0 0</pose>
        <visualize>true</visualize>
        <update_rate>10</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>${-horizontal_fov/360*M_PI}</min_angle>
				<max_angle>${horizontal_fov/360*M_PI}</max_angle>
				</horizontal>
				<vertical>
				<samples>50</samples>
				<resolution>1</resolution>
				<min_angle>${-vertical_fov/360*M_PI}</min_angle>
				<max_angle>${vertical_fov/360*M_PI}</max_angle>
				</vertical>
			  </scan>
			  <range>
				<min>0.1</min>
				<max>200</max>
				<resolution>0.002</resolution>
			  </range>
			  <noise>
				<type>gaussian</type>
				<mean>0.0</mean>
				<stddev>0.01</stddev>
			  </noise>
			</ray>
          <visualize>true</visualize>
		  <samples>10000</samples>
		  <downsample>1</downsample>
		  <csv_file_name>package://livox_laser_simulation/scan_mode/mid70.csv</csv_file_name>
		  <ros_topic>/scan</ros_topic>
        </plugin>
      </sensor>
  </gazebo>

看到上面这一长串的标签型代码块,是不是内心来了个握草三连,这是啥?咋写的?为啥是这样?
别急,听我说,谢谢ni~不好意思,走错片场了。上面这一段其实就是livox_laser_simulation里的内容,具体文件是这里面的/livox_laser_simulation/urdf/livox_mid70.xacro。所以,只是把仿真包里面的核心代码复制过来了,至于这些参数,可以根据不同型号的雷达进行相应的调整。这里面把小方块和解释它身份的标签联系起来的是这句:

<gazebo reference="laser_livox">

到这里,这篇文章基本上就结束啦。最后贴上完整的小车urdf描述文件,供大家参考:

<?xml version="1.0"?>
<robot name="husky" xmlns:xacro="http://ros.org/wiki/xacro">
  <!-- IMU Link -->
  <xacro:arg name="imu_xyz"     default="$(optenv HUSKY_IMU_XYZ 0.19 0 0.149)"/>
  <xacro:arg name="imu_rpy"     default="$(optenv HUSKY_IMU_RPY 0 -1.5708 3.1416)"/>
  <xacro:arg name="imu_parent"  default="$(optenv HUSKY_IMU_PARENT base_link)"/>

  <!-- LMS1XX Laser Primary and Secondary -->
  <xacro:arg name="laser_enabled"           default="$(optenv HUSKY_LMS1XX_ENABLED 0)" />
  <xacro:arg name="laser_topic"             default="$(optenv HUSKY_LMS1XX_TOPIC front/scan)"/>
  <xacro:arg name="laser_prefix"            default="$(optenv HUSKY_LMS1XX_PREFIX front)"/>
  <xacro:arg name="laser_parent"            default="$(optenv HUSKY_LMS1XX_PARENT top_plate_link)"/>
  <xacro:arg name="laser_xyz"               default="$(optenv HUSKY_LMS1XX_XYZ 0.2206 0.0 0.00635)" />
  <xacro:arg name="laser_rpy"               default="$(optenv HUSKY_LMS1XX_RPY 0.0 0.0 0.0)" />

  <xacro:arg name="laser_secondary_enabled" default="$(optenv HUSKY_LMS1XX_SECONDARY_ENABLED 0)" />
  <xacro:arg name="laser_secondary_topic"   default="$(optenv HUSKY_LMS1XX_SECONDARY_TOPIC rear/scan)"/>
  <xacro:arg name="laser_secondary_prefix"  default="$(optenv HUSKY_LMS1XX_SECONDARY_PREFIX rear)"/>
  <xacro:arg name="laser_secondary_parent"  default="$(optenv HUSKY_LMS1XX_SECONDARY_PARENT top_plate_link)"/>
  <xacro:arg name="laser_secondary_xyz"     default="$(optenv HUSKY_LMS1XX_SECONDARY_XYZ -0.2206 0.0 0.00635)" />
  <xacro:arg name="laser_secondary_rpy"     default="$(optenv HUSKY_LMS1XX_SECONDARY_RPY 0.0 0.0 3.14159)" />

  <!-- UST10 Laser Primary  and Secondary -->
  <xacro:arg name="laser_ust10_front_enabled" default="$(optenv HUSKY_UST10_ENABLED 0)" />
  <xacro:arg name="laser_ust10_front_topic"   default="$(optenv HUSKY_UST10_TOPIC front/scan)" />
  <xacro:arg name="laser_ust10_front_prefix"  default="$(optenv HUSKY_UST10_PREFIX front)" />
  <xacro:arg name="laser_ust10_front_parent"  default="$(optenv HUSKY_UST10_PARENT top_plate_link)" />
  <xacro:arg name="laser_ust10_front_xyz"     default="$(optenv HUSKY_UST10_XYZ 0.2206 0.0 0.00635)" />
  <xacro:arg name="laser_ust10_front_rpy"     default="$(optenv HUSKY_UST10_RPY 0 0 0)" />

  <xacro:arg name="laser_ust10_rear_enabled"  default="$(optenv HUSKY_UST10_SECONDARY_ENABLED 0)" />
  <xacro:arg name="laser_ust10_rear_topic"    default="$(optenv HUSKY_UST10_SECONDARY_TOPIC rear/scan)" />
  <xacro:arg name="laser_ust10_rear_prefix"   default="$(optenv HUSKY_UST10_SECONDARY_PREFIX rear)" />
  <xacro:arg name="laser_ust10_rear_parent"   default="$(optenv HUSKY_UST10_SECONDARY_PARENT top_plate_link)" />
  <xacro:arg name="laser_ust10_rear_xyz"      default="$(optenv HUSKY_UST10_SECONDARY_XYZ -0.2206 0.0 0.00635)" />
  <xacro:arg name="laser_ust10_rear_rpy"      default="$(optenv HUSKY_UST10_SECONDARY_RPY 0 0 3.14159)" />

  <!-- Velodyne LiDAR Primary and Secondary -->
  <xacro:arg name="laser_3d_enabled"              default="$(optenv HUSKY_LASER_3D_ENABLED 0)" />
  <xacro:arg name="laser_3d_topic"                default="$(optenv HUSKY_LASER_3D_TOPIC points)"/>
  <xacro:arg name="laser_3d_tower"                default="$(optenv HUSKY_LASER_3D_TOWER 1)"/>
  <xacro:arg name="laser_3d_prefix"               default="$(optenv HUSKY_LASER_3D_PREFIX )"/>
  <xacro:arg name="laser_3d_parent"               default="$(optenv HUSKY_LASER_3D_PARENT top_plate_link)"/>
  <xacro:arg name="laser_3d_xyz"                  default="$(optenv HUSKY_LASER_3D_XYZ 0 0 0)" />
  <xacro:arg name="laser_3d_rpy"                  default="$(optenv HUSKY_LASER_3D_RPY 0 0 0)" />

  <xacro:arg name="laser_3d_secondary_enabled"    default="$(optenv HUSKY_LASER_3D_SECONDARY_ENABLED 0)" />
  <xacro:arg name="laser_3d_secondary_topic"      default="$(optenv HUSKY_LASER_3D_SECONDARY_TOPIC secondary_points)"/>
  <xacro:arg name="laser_3d_secondary_tower"      default="$(optenv HUSKY_LASER_3D_SECONDARY_TOWER 1)"/>
  <xacro:arg name="laser_3d_secondary_prefix"     default="$(optenv HUSKY_LASER_3D_SECONDARY_PREFIX secondary_)"/>
  <xacro:arg name="laser_3d_secondary_parent"     default="$(optenv HUSKY_LASER_3D_SECONDARY_PARENT top_plate_link)"/>
  <xacro:arg name="laser_3d_secondary_xyz"        default="$(optenv HUSKY_LASER_3D_SECONDARY_XYZ 0 0 0)" />
  <xacro:arg name="laser_3d_secondary_rpy"        default="$(optenv HUSKY_LASER_3D_SECONDARY_RPY 0 0 -3.14159)" />

  <!-- Realsense Camera Primary and Secondary -->
  <xacro:arg name="realsense_enabled"             default="$(optenv HUSKY_REALSENSE_ENABLED 0)" />
  <xacro:arg name="realsense_topic"               default="$(optenv HUSKY_REALSENSE_TOPIC realsense)" />
  <xacro:arg name="realsense_prefix"              default="$(optenv HUSKY_REALSENSE_PREFIX camera)" />
  <xacro:arg name="realsense_parent"              default="$(optenv HUSKY_REALSENSE_PARENT top_plate_link)" />
  <xacro:arg name="realsense_xyz"                 default="$(optenv HUSKY_REALSENSE_XYZ 0 0 0)" />
  <xacro:arg name="realsense_rpy"                 default="$(optenv HUSKY_REALSENSE_RPY 0 0 0)" />

  <xacro:arg name="realsense_secondary_enabled"   default="$(optenv HUSKY_REALSENSE_SECONDARY_ENABLED 0)" />
  <xacro:arg name="realsense_secondary_topic"     default="$(optenv HUSKY_REALSENSE_SECONDARY_TOPIC realsense_secondary)" />
  <xacro:arg name="realsense_secondary_prefix"    default="$(optenv HUSKY_REALSENSE_SECONDARY_PREFIX camera_secondary)" />
  <xacro:arg name="realsense_secondary_parent"    default="$(optenv HUSKY_REALSENSE_SECONDARY_PARENT top_plate_link)" />
  <xacro:arg name="realsense_secondary_xyz"       default="$(optenv HUSKY_REALSENSE_SECONDARY_XYZ 0 0 0)" />
  <xacro:arg name="realsense_secondary_rpy"       default="$(optenv HUSKY_REALSENSE_SECONDARY_RPY 0 0 0)" />

  <!-- BlackflyS Camera Primary and Secondary -->
  <xacro:arg name="blackfly_enabled"                  default="$(optenv HUSKY_BLACKFLY 0)"/>
  <xacro:arg name="blackfly_mount_enabled"            default="$(optenv HUSKY_BLACKFLY_MOUNT_ENABLED 1)"/>
  <xacro:arg name="blackfly_mount_angle"              default="$(optenv HUSKY_BLACKFLY_MOUNT_ANGLE 0)"/>
  <xacro:arg name="blackfly_prefix"                   default="$(optenv HUSKY_BLACKFLY_PREFIX blackfly)"/>
  <xacro:arg name="blackfly_parent"                   default="$(optenv HUSKY_BLACKFLY_PARENT top_plate_link)"/>
  <xacro:arg name="blackfly_xyz"                      default="$(optenv HUSKY_BLACKFLY_XYZ 0 0 0)"/>
  <xacro:arg name="blackfly_rpy"                      default="$(optenv HUSKY_BLACKFLY_RPY 0 0 0)"/>

  <xacro:arg name="blackfly_secondary_enabled"        default="$(optenv HUSKY_BLACKFLY_SECONDARY 0)"/>
  <xacro:arg name="blackfly_secondary_mount_enabled"  default="$(optenv HUSKY_BLACKFLY_SECONDARY_MOUNT_ENABLED 1)"/>
  <xacro:arg name="blackfly_secondary_mount_angle"    default="$(optenv HUSKY_BLACKFLY_SECONDARY_MOUNT_ANGLE 0)"/>
  <xacro:arg name="blackfly_secondary_prefix"         default="$(optenv HUSKY_BLACKFLY_SECONDARY_PREFIX blackfly_secondary)"/>
  <xacro:arg name="blackfly_secondary_parent"         default="$(optenv HUSKY_BLACKFLY_SECONDARY_PARENT top_plate_link)"/>
  <xacro:arg name="blackfly_secondary_xyz"            default="$(optenv HUSKY_BLACKFLY_SECONDARY_XYZ 0 0 0)"/>
  <xacro:arg name="blackfly_secondary_rpy"            default="$(optenv HUSKY_BLACKFLY_SECONDARY_RPY 0 0 0)"/>

  <!-- Bumper Extension -->
  <xacro:property name="husky_front_bumper_extend"  value="$(optenv HUSKY_FRONT_BUMPER_EXTEND 0)" />
  <xacro:property name="husky_rear_bumper_extend"   value="$(optenv HUSKY_REAR_BUMPER_EXTEND 0)" />

  <!-- Height of the sensor arch in mm.  Must be either 510 or 300 -->
  <xacro:arg name="sensor_arch"         default="$(optenv HUSKY_SENSOR_ARCH 0)" />
  <xacro:arg name="sensor_arch_height"  default="$(optenv HUSKY_SENSOR_ARCH_HEIGHT 510)" />
  <xacro:arg name="sensor_arch_xyz"     default="$(optenv HUSKY_SENSOR_ARCH_OFFSET 0 0 0)"/>
  <xacro:arg name="sensor_arch_rpy"     default="$(optenv HUSKY_SENSOR_ARCH_RPY 0 0 0)"/>

  <!-- Extras -->
  <xacro:arg name="robot_namespace" default="$(optenv ROBOT_NAMESPACE /)" />
  <xacro:arg name="urdf_extras"     default="$(optenv HUSKY_URDF_EXTRAS empty.urdf)" />
  <xacro:arg name="cpr_urdf_extras" default="$(optenv CPR_URDF_EXTRAS empty.urdf)" />

  <!-- Included URDF/XACRO Files -->
  <xacro:include filename="$(find husky_description)/urdf/accessories/hokuyo_ust10.urdf.xacro" />
  <xacro:include filename="$(find husky_description)/urdf/accessories/intel_realsense.urdf.xacro"/>
  <xacro:include filename="$(find husky_description)/urdf/accessories/flir_blackfly_mount.urdf.xacro"/>
  <xacro:include filename="$(find husky_description)/urdf/accessories/sensor_arch.urdf.xacro"/>
  <xacro:include filename="$(find husky_description)/urdf/accessories/sick_lms1xx_mount.urdf.xacro"/>
  <xacro:include filename="$(find husky_description)/urdf/accessories/vlp16_mount.urdf.xacro"/>
  <xacro:include filename="$(find husky_description)/urdf/decorations.urdf.xacro" />
  <xacro:include filename="$(find husky_description)/urdf/wheel.urdf.xacro" />
  

  <xacro:property name="M_PI" value="3.14159"/>

  <!-- Base Size -->
  <xacro:property name="base_x_size" value="0.98740000" />
  <xacro:property name="base_y_size" value="0.57090000" />
  <xacro:property name="base_z_size" value="0.24750000" />

  <!-- Wheel Mounting Positions -->
  <xacro:property name="wheelbase" value="0.5120" />
  <xacro:property name="track" value="0.5708" />
  <xacro:property name="wheel_vertical_offset" value="0.03282" />

  <!-- Wheel Properties -->
  <xacro:property name="wheel_length" value="0.1143" />
  <xacro:property name="wheel_radius" value="0.1651" />

  <!-- Base link is the center of the robot's bottom plate -->
  <link name="base_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://husky_description/meshes/base_link.dae" />
      </geometry>
    </visual>
    <collision>
      <origin xyz="${( husky_front_bumper_extend - husky_rear_bumper_extend ) / 2.0} 0 ${base_z_size/4}" rpy="0 0 0" />
      <geometry>
        <box size="${ base_x_size + husky_front_bumper_extend + husky_rear_bumper_extend } ${base_y_size} ${base_z_size/2}"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0 ${base_z_size*3/4-0.01}" rpy="0 0 0" />
      <geometry>
        <box size="${base_x_size*4/5} ${base_y_size} ${base_z_size/2-0.02}"/>
      </geometry>
    </collision>
  </link>

  <!-- Base footprint is on the ground under the robot -->
  <link name="base_footprint"/>

  <joint name="base_footprint_joint" type="fixed">
    <origin xyz="0 0 ${wheel_vertical_offset - wheel_radius}" rpy="0 0 0" />
    <parent link="base_link" />
    <child link="base_footprint" />
  </joint>

  <!-- Inertial link stores the robot's inertial information -->
  <link name="inertial_link">
    <inertial>
      <mass value="46.034" />
      <origin xyz="-0.00065 -0.085 0.062" />
      <inertia ixx="0.6022" ixy="-0.02364" ixz="-0.1197" iyy="1.7386" iyz="-0.001544" izz="2.0296" />
    </inertial>
  </link>

  <joint name="inertial_joint" type="fixed">
    <origin xyz="0 0 0" rpy="0 0 0" />
    <parent link="base_link" />
    <child link="inertial_link" />
  </joint>

  <!-- Husky wheel macros -->
  <xacro:husky_wheel wheel_prefix="front_left">
    <origin xyz="${wheelbase/2} ${track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
  </xacro:husky_wheel>
  <xacro:husky_wheel wheel_prefix="front_right">
    <origin xyz="${wheelbase/2} ${-track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
  </xacro:husky_wheel>
  <xacro:husky_wheel wheel_prefix="rear_left">
    <origin xyz="${-wheelbase/2} ${track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
  </xacro:husky_wheel>
  <xacro:husky_wheel wheel_prefix="rear_right">
    <origin xyz="${-wheelbase/2} ${-track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
  </xacro:husky_wheel>

  <!-- Husky Decorations -->
  <xacro:husky_decorate />

  <!--
    Add the main sensor arch if the user has specifically enabled it, or if a sensor
    requires it for mounting
  -->
  <xacro:if value="$(arg sensor_arch)">
    <xacro:sensor_arch prefix="" parent="top_plate_link" size="$(arg sensor_arch_height)">
      <origin xyz="$(arg sensor_arch_xyz)" rpy="$(arg sensor_arch_rpy)"/>
    </xacro:sensor_arch>
  </xacro:if>


  <!--
    SICK LMS1XX Priamry and Secondary Laser Scans
  -->
  <xacro:if value="$(arg laser_enabled)">
    <xacro:sick_lms1xx_mount prefix="$(arg laser_prefix)"/>
    <xacro:sick_lms1xx frame="$(arg laser_prefix)_laser" topic="$(arg laser_topic)" robot_namespace="$(arg robot_namespace)"/>

    <joint name="$(arg laser_prefix)_laser_mount_joint" type="fixed">
      <origin xyz="$(arg laser_xyz)" rpy="$(arg laser_rpy)" />
      <parent link="$(arg laser_parent)" />
      <child link="$(arg laser_prefix)_laser_mount" />
    </joint>
  </xacro:if>

  <xacro:if value="$(arg laser_secondary_enabled)">
    <xacro:sick_lms1xx_mount prefix="$(arg laser_secondary_prefix)"/>
    <xacro:sick_lms1xx frame="$(arg laser_secondary_prefix)_laser" topic="$(arg laser_secondary_topic)" robot_namespace="$(arg robot_namespace)"/>

    <joint name="$(arg laser_secondary_prefix)_laser_mount_joint" type="fixed">
      <origin xyz="$(arg laser_secondary_xyz)" rpy="$(arg laser_secondary_rpy)" />
      <parent link="$(arg laser_secondary_parent)" />
      <child link="$(arg laser_secondary_prefix)_laser_mount" />
    </joint>
  </xacro:if>

  <!--
    Hokuyo UST10 Primary and Secondary Laser Scans
  -->
  <xacro:if value="$(arg laser_ust10_front_enabled)">
    <xacro:hokuyo_ust10_mount topic="$(arg laser_ust10_front_topic)" prefix="$(arg laser_ust10_front_prefix)" parent_link="$(arg laser_ust10_front_parent)">
        <origin xyz="$(arg laser_ust10_front_xyz)" rpy="$(arg laser_ust10_front_rpy)" />
      </xacro:hokuyo_ust10_mount>
  </xacro:if>

  <xacro:if value="$(arg laser_ust10_rear_enabled)">
    <xacro:hokuyo_ust10_mount topic="$(arg laser_ust10_rear_topic)" prefix="$(arg laser_ust10_rear_prefix)" parent_link="$(arg laser_ust10_rear_parent)">
        <origin xyz="$(arg laser_ust10_rear_xyz)" rpy="$(arg laser_ust10_rear_rpy)" />
      </xacro:hokuyo_ust10_mount>
  </xacro:if>

  <!-- Intel Realsense Primary and Secondary -->
  <xacro:if value="$(arg realsense_enabled)">
    <link name="$(arg realsense_prefix)_realsense_mountpoint"/>
    <joint name="$(arg realsense_prefix)_realsense_mountpoint_joint" type="fixed">
      <origin xyz="$(arg realsense_xyz)" rpy="$(arg realsense_rpy)" />
      <parent link="$(arg realsense_parent)"/>
      <child link="$(arg realsense_prefix)_realsense_mountpoint" />
    </joint>
    <xacro:intel_realsense_mount prefix="$(arg realsense_prefix)" topic="$(arg realsense_topic)" parent_link="$(arg realsense_prefix)_realsense_mountpoint"/>
  </xacro:if>

  <xacro:if value="$(arg realsense_secondary_enabled)">
    <link name="$(arg realsense_secondary_prefix)_realsense_mountpoint"/>
    <joint name="$(arg realsense_secondary_prefix)_realsense_mountpoint_joint" type="fixed">
      <origin xyz="$(arg realsense_secondary_xyz)" rpy="$(arg realsense_secondary_rpy)" />
      <parent link="$(arg realsense_secondary_parent)"/>
      <child link="$(arg realsense_secondary_prefix)_realsense_mountpoint" />
    </joint>
    <xacro:intel_realsense_mount prefix="$(arg realsense_secondary_prefix)" topic="$(arg realsense_secondary_topic)" parent_link="$(arg realsense_secondary_prefix)_realsense_mountpoint"/>
  </xacro:if>

  <!-- BlackflyS Camera Primary and Secondary -->
  <xacro:if value="$(arg blackfly_enabled)">
    <xacro:flir_blackfly_mount prefix="$(arg blackfly_prefix)"
                                parent="$(arg blackfly_parent)"
                                mount_enabled="$(arg blackfly_mount_enabled)"
                                mount_angle="$(arg blackfly_mount_angle)">
      <origin xyz="$(arg blackfly_xyz)" rpy="$(arg blackfly_rpy)"/>
    </xacro:flir_blackfly_mount>
  </xacro:if>

  <xacro:if value="$(arg blackfly_secondary_enabled)">
    <xacro:flir_blackfly_mount prefix="$(arg blackfly_secondary_prefix)"
                                parent="$(arg blackfly_secondary_parent)"
                                mount_enabled="$(arg blackfly_secondary_mount_enabled)"
                                mount_angle="$(arg blackfly_secondary_mount_angle)">
      <origin xyz="$(arg blackfly_secondary_xyz)" rpy="$(arg blackfly_secondary_rpy)"/>
    </xacro:flir_blackfly_mount>
  </xacro:if>



<!--******************* add VLP laser Link *************************************-->
  <joint name="VLP16_base_mount_joint" type="fixed">
    <origin xyz="0 0.0 0.25" rpy="0.0 0.0 0.0"/>
    <parent link="base_link"/>
    <child link="VLP16_base_link"/>
  </joint>

  <link name="VLP16_base_link">
    <visual>
      <geometry>
        <mesh filename="package://velodyne_description/meshes/VLP16_base_1.dae" />
      </geometry>
    </visual>
    <visual>
      <geometry>
        <mesh filename="package://velodyne_description/meshes/VLP16_base_2.dae" />
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.03585"/>
      <geometry>
        <cylinder radius="0.0516" length="0.0717"/>
      </geometry>
    </collision>
  </link>

  <joint name="VLP16_base_scan_joint" type="fixed" >
    <origin xyz="0 0 0.0377" rpy="0 0 0" />
    <parent link="VLP16_base_link" />
    <child link="velodyne"/>
  </joint>

  <link name="velodyne">
    <inertial>
      <mass value="0.01"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="1e-7" ixy="0" ixz="0" iyy="1e-7" iyz="0" izz="1e-7"/>
    </inertial>
    <visual>
      <origin xyz="0 0 -0.0377" />
      <geometry>
        <mesh filename="package://velodyne_description/meshes/VLP16_scan.dae" />
      </geometry>
    </visual>
  </link>
  
  <link name="laser_livox">
	<collision>
	  <origin xyz="0 0 0" rpy="0 0 0"/>  
	  <geometry>
		<box size="0.1 0.1 0.1"/>
	  </geometry>
	</collision>	
	<visual>
	  <origin xyz="0 0 0" rpy="0 0 0"/>
	  <geometry>
		<box size="0.1 0.1 0.1"/>
	  </geometry>
	</visual>
  </link>
  
  <joint name="laser_livox_joint" type="fixed" >
    <origin xyz="0 0 0.5" rpy="0 0 0" />
    <parent link="base_link" />
    <child link="laser_livox"/>
  </joint>

  <!--*********************************************************************-->

  <!--*********************************************************************-->

  <!--  add imu llink   -->
  <link name="imu_link">
    <visual>
      <geometry>
        <box size="0.015 0.015 0.007"/>
      </geometry>
    </visual>
  </link>
  <joint name ="imu_joints" type="fixed">
    <origin xyz="-0.10 0.0 0.02" rpy="0.0 0.0 0.0"/>
    <parent link="base_link"/>
    <child link="imu_link"/>
  </joint>
  <!--*********************************************************************-->


  <!-- add laser simulation plugines adaptored for rplidar A1M8 -->
  <gazebo reference="velodyne">
    <sensor type="ray" name="velodyne-VLP16">      <!-- ray   -->   <!-- gpu_ray -->
      <pose>0 0 0 0 0 0</pose>
      <visualize>false</visualize>
      <update_rate>10</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>1875</samples>
            <resolution>1</resolution>
            <min_angle>0.9</min_angle>
            <max_angle>130</max_angle>
          </horizontal>
          <vertical>
            <samples>16</samples>
            <resolution>1</resolution>
            <min_angle>-0.26</min_angle>   <!-- 15 -->
            <max_angle> 0.26</max_angle>
          </vertical>
        </scan>
        <range>
          <min>0.9</min>
          <max>131</max>
          <resolution>0.001</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.0</stddev>
        </noise>
      </ray>
      <plugin name="gazebo_ros_laser_controller" filename="libgazebo_ros_velodyne_laser.so">     <!-- libgazebo_ros_velodyne_gpu_laser  --> <!-- libgazebo_ros_velodyne_laser  -->
        <topicName>/velodyne_points</topicName>
        <frameName>velodyne</frameName>
        <organize_cloud>false</organize_cloud>
        <min_range>0.9</min_range>
        <max_range>130</max_range>
        <gaussianNoise>0.008</gaussianNoise>
      </plugin>
    </sensor>
  </gazebo>
  <!--*********************************************************************-->
  
  
  <xacro:property name="horizontal_fov" value="70.4"/>
  <xacro:property name="vertical_fov" value="70.4"/>
  <gazebo reference="laser_livox">
      <sensor type="ray" name="laser_livox">
        <pose>0 0 0 0 0 0</pose>
        <visualize>true</visualize>
        <update_rate>10</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>${-horizontal_fov/360*M_PI}</min_angle>
				<max_angle>${horizontal_fov/360*M_PI}</max_angle>
				</horizontal>
				<vertical>
				<samples>50</samples>
				<resolution>1</resolution>
				<min_angle>${-vertical_fov/360*M_PI}</min_angle>
				<max_angle>${vertical_fov/360*M_PI}</max_angle>
				</vertical>
			  </scan>
			  <range>
				<min>0.1</min>
				<max>200</max>
				<resolution>0.002</resolution>
			  </range>
			  <noise>
				<type>gaussian</type>
				<mean>0.0</mean>
				<stddev>0.01</stddev>
			  </noise>
			</ray>
          <visualize>true</visualize>
		  <samples>10000</samples>
		  <downsample>1</downsample>
		  <csv_file_name>package://livox_laser_simulation/scan_mode/mid70.csv</csv_file_name>
		  <ros_topic>/scan</ros_topic>
        </plugin>
      </sensor>
    </gazebo>
    
    <gazebo>
      <plugin name="p3d_base_controller" filename="libgazebo_ros_p3d.so">
        <alwaysOn>true</alwaysOn>
        <updateRate>100.0</updateRate>
        <bodyName>velodyne</bodyName>
        <topicName>/ground_truth</topicName>
        <gaussianNoise>0.0</gaussianNoise>
        <frameName>map</frameName>
        <xyzOffsets>0 0 0</xyzOffsets>
        <rpyOffsets>0 0 0</rpyOffsets>
      </plugin>
    </gazebo>
  

  <!-- add imu link simlations plugines  -->
  <gazebo reference="imu_link">
    <gravity>true</gravity>
    <sensor name="imu_sensor" type="imu">
      <always_on>true</always_on>
      <update_rate>100</update_rate>
      <visualize>false</visualize>
      <topic>/imu/data</topic>
      <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
        <topicName>/imu/data</topicName>
        <bodyName>imu_link</bodyName>
        <updateRateHZ>100.0</updateRateHZ>
        <gaussianNoise>0.0</gaussianNoise>
        <xyzOffset>0 0 0</xyzOffset>
        <rpyOffset>0 0 0</rpyOffset>
        <frameName>imu_link</frameName>
        <initialOrientationAsReference>false</initialOrientationAsReference>
      </plugin>
      <pose>0 0 0 0 0 0</pose>
    </sensor>
  </gazebo>
  <!--*********************************************************************-->

  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>$(arg robot_namespace)</robotNamespace>
      <legacyModeNS>true</legacyModeNS>
    </plugin>
  </gazebo>

  <gazebo>
    <plugin name="imu_controller" filename="libhector_gazebo_ros_imu.so">
      <robotNamespace>$(arg robot_namespace)</robotNamespace>
      <updateRate>100.0</updateRate>
      <bodyName>base_link</bodyName>
      <topicName>imu/data</topicName>
      <accelDrift>0.005 0.005 0.005</accelDrift>
      <accelGaussianNoise>0.005 0.005 0.005</accelGaussianNoise>
      <rateDrift>0.005 0.005 0.005 </rateDrift>
      <rateGaussianNoise>0.005 0.005 0.005 </rateGaussianNoise>
      <headingDrift>0.005</headingDrift>
      <headingGaussianNoise>0.005</headingGaussianNoise>
    </plugin>
  </gazebo>

  <gazebo>
    <plugin name="gps_controller" filename="libhector_gazebo_ros_gps.so">
      <robotNamespace>$(arg robot_namespace)</robotNamespace>
      <updateRate>40</updateRate>
      <bodyName>base_link</bodyName>
      <frameId>base_link</frameId>
      <topicName>navsat/fix</topicName>
      <velocityTopicName>navsat/vel</velocityTopicName>
      <referenceLatitude>49.9</referenceLatitude>
      <referenceLongitude>8.9</referenceLongitude>
      <referenceHeading>0</referenceHeading>
      <referenceAltitude>0</referenceAltitude>
      <drift>0.0001 0.0001 0.0001</drift>
    </plugin>
  </gazebo>

  <!-- Optional custom includes. -->
  <xacro:include filename="$(arg urdf_extras)" />

  <!-- Optional for Clearpath internal softwares -->
  <xacro:include filename="$(arg cpr_urdf_extras)" />

</robot>

为了节省篇幅,把文件里面的许可信息略去了。

四、参考文献

[1] ROS仿真笔记之——gazebo配置velodyne(引用CSDN博主:gwpscut)
[2] [gazebo仿真给模型添加多线激光雷达(引用CSDN博主:Travis.X)

你可能感兴趣的:(SLAM-ROS,机器人,ubuntu,自动驾驶)