搭建自己的 3D SLAM 仿真（1）搭建仿真环境

本文仿真采用turtlebot3系列的小车和velodyne-16线雷达组合进行仿真

1.下载安装turtlebot3和velodyne-16线雷达

1.1 turtlebot3

sudo apt-get install ros-noetic-turtlebot3-*

具体内容可自行搜索

1.2 velodyne-16

clone源码到在src下

git clone https://github.com/lmark1/velodyne_simulator.git

sudo apt-get install ros-kinetic-velodyne-*

 运行实例文件，检查3D雷达（加载较慢，如果卡住自行搜索解决方案）

roslaunch velodyne_description example.launch 

 gazebo仿真环境和rviz显示如下：

2.组合小车和3D雷达

在opt/ros/*/share/turtlebot3_description/urdf拷贝一份到自己的环境中进行修改，其中velodyne_simulator中存放的是雷达的相关文件，注意1在雷达的<inertial>内容注释，由于搭建的简易小车没考虑力学相关问题，雷达重量可能会导致小车运动时候发生侧翻

注意2，要修改这部分的gpu参数，只有设置为gpu时候才能检测动态障碍物

  <xacro:include filename="$(find 3d_slam_simulation)/urdf/VLP-16.urdf.xacro"/>
  <xacro:VLP-16 parent="base_link" name="velodyne" topic="/velodyne_points" hz="10" samples="440" gpu="false">
    <origin xyz="0 0 0.2" rpy="0 0 0" />
  </xacro:VLP-16>

（我的仅作参考，根据文件中example文件分别进行）在你自己环境下的：

launch文件夹下创建wpb_tur_vlp16.launch

<?xml version="1.0" ?>
<launch>
  
  <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find wpr_simulation)/worlds/robocup_home.world"/>
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="gui" value="true"/>
    <arg name="recording" value="false"/>
    <arg name="debug" value="false"/>
  </include>

  <!-- Define robot description once -->

  <!-- Spawn the second robot model (wpb_home) -->
  <!-- <arg name="model" default="$(find wpr_simulation)/urdf/tur_16.xacro"/>
  <param name="robot_description" command="$(find xacro)/xacro $(arg model)" />
  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model"
        args="-param robot_description -urdf -model wpb_home" /> -->


    <arg name="gpu" default="false"/>
  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find wpr_simulation)/urdf/tur_16.xacro' gpu:=$(arg gpu)" />
  
  <node pkg="gazebo_ros" type="spawn_model" name="spawn_model" args="-urdf -param /robot_description -model example"/>
  

  <!-- tf node for robot state -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
    <param name="publish_frequency" type="double" value="30.0" />
  </node>

  <!-- Joint state publisher -->
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />

  <!-- Lidar filter node -->
  <node pkg="wpr_simulation" type="lidar_filter" name="lidar_filter" />

  <!-- RViz -->
  <arg name="rviz" default="true"/>
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="$(anon rviz)" respawn="false" output="screen" args="-d $(find wpr_simulation)/rviz/tur_vlp16.rviz" />


</launch>

urdf文件夹下创建tur_16.xacro

<?xml version="1.0" ?>
<robot name="turtlebot3_waffle" xmlns:xacro="http://ros.org/wiki/xacro">
  <xacro:arg name="gpu" default="false"/>

  <xacro:include filename="$(find turtlebot3_description)/urdf/common_properties.xacro"/>
  <xacro:include filename="$(find turtlebot3_description)/urdf/turtlebot3_waffle.gazebo.xacro"/>

  <xacro:property name="r200_cam_rgb_px" value="0.005"/>
  <xacro:property name="r200_cam_rgb_py" value="0.018"/>
  <xacro:property name="r200_cam_rgb_pz" value="0.013"/>
  <xacro:property name="r200_cam_depth_offset" value="0.01"/>

  <link name="base_footprint"/>

  <joint name="base_joint" type="fixed">
    <parent link="base_footprint"/>
    <child link="base_link" />
    <origin xyz="0 0 0.010" rpy="0 0 0"/>
  </joint>

  <link name="base_link">
    <visual>
      <origin xyz="-0.064 0 0.0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="package://turtlebot3_description/meshes/bases/waffle_base.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="light_black"/>
    </visual>

    <collision>
      <origin xyz="-0.064 0 0.047" rpy="0 0 0"/>
      <geometry>
        <box size="0.266 0.266 0.094"/>
      </geometry>
    </collision>
    
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="1.3729096e+00"/>
      <inertia ixx="8.7002718e-03" ixy="-4.7576583e-05" ixz="1.1160499e-04"
               iyy="8.6195418e-03" iyz="-3.5422299e-06"
               izz="1.4612727e-02" />
    </inertial>
  </link>

  <joint name="wheel_left_joint" type="continuous">
    <parent link="base_link"/>
    <child link="wheel_left_link"/>
    <origin xyz="0.0 0.144 0.023" rpy="-1.57 0 0"/>
    <axis xyz="0 0 1"/>
  </joint>

  <link name="wheel_left_link">
    <visual>
      <origin xyz="0 0 0" rpy="1.57 0 0"/>
      <geometry>
        <mesh filename="package://turtlebot3_description/meshes/wheels/left_tire.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark"/>
    </visual>

    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.018" radius="0.033"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 0" />
      <mass value="2.8498940e-02" />
      <inertia ixx="1.1175580e-05" ixy="-4.2369783e-11" ixz="-5.9381719e-09"
               iyy="1.1192413e-05" iyz="-1.4400107e-11"
               izz="2.0712558e-05" />
      </inertial>
  </link>

  <joint name="wheel_right_joint" type="continuous">
    <parent link="base_link"/>
    <child link="wheel_right_link"/>
    <origin xyz="0.0 -0.144 0.023" rpy="-1.57 0 0"/>
    <axis xyz="0 0 1"/>
  </joint>

  <link name="wheel_right_link">
    <visual>
      <origin xyz="0 0 0" rpy="1.57 0 0"/>
      <geometry>
        <mesh filename="package://turtlebot3_description/meshes/wheels/right_tire.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark"/>
    </visual>

    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.018" radius="0.033"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 0" />
      <mass value="2.8498940e-02" />
      <inertia ixx="1.1175580e-05" ixy="-4.2369783e-11" ixz="-5.9381719e-09"
               iyy="1.1192413e-05" iyz="-1.4400107e-11"
               izz="2.0712558e-05" />
      </inertial>
  </link>

  <joint name="caster_back_right_joint" type="fixed">
    <parent link="base_link"/>
    <child link="caster_back_right_link"/>
    <origin xyz="-0.177 -0.064 -0.004" rpy="-1.57 0 0"/>
  </joint>

  <link name="caster_back_right_link">
    <collision>
      <origin xyz="0 0.001 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.030 0.009 0.020"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 0" />
      <mass value="0.005" />
      <inertia ixx="0.001" ixy="0.0" ixz="0.0"
               iyy="0.001" iyz="0.0"
               izz="0.001" />
    </inertial>
  </link>

  <joint name="caster_back_left_joint" type="fixed">
    <parent link="base_link"/>
    <child link="caster_back_left_link"/>
    <origin xyz="-0.177 0.064 -0.004" rpy="-1.57 0 0"/>
  </joint>

  <link name="caster_back_left_link">
    <collision>
      <origin xyz="0 0.001 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.030 0.009 0.020"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 0" />
      <mass value="0.005" />
      <inertia ixx="0.001" ixy="0.0" ixz="0.0"
               iyy="0.001" iyz="0.0"
               izz="0.001" />
    </inertial>
  </link>

  <joint name="imu_joint" type="fixed">
    <parent link="base_link"/>
    <child link="imu_link"/>
    <origin xyz="0.0 0 0.068" rpy="0 0 0"/>
  </joint>

  <link name="imu_link"/>

  <joint name="scan_joint" type="fixed">
    <parent link="base_link"/>
    <child link="base_scan"/>
    <origin xyz="-0.064 0 0.122" rpy="0 0 0"/>
  </joint>

  <link name="base_scan">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="package://turtlebot3_description/meshes/sensors/lds.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark"/>
    </visual>

    <collision>
      <origin xyz="0.015 0 -0.0065" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.0315" radius="0.055"/>
      </geometry>
    </collision>

    <inertial>
      <mass value="0.114" />
      <origin xyz="0 0 0" />
      <inertia ixx="0.001" ixy="0.0" ixz="0.0"
               iyy="0.001" iyz="0.0"
               izz="0.001" />
    </inertial>
  </link>

  <joint name="camera_joint" type="fixed">
    <origin xyz="0.064 -0.065 0.094" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="camera_link"/>
  </joint>

  <link name="camera_link">
    <visual>
     <origin xyz="0 0 0" rpy="1.57 0 1.57"/>
      <geometry>
       <mesh filename="package://turtlebot3_description/meshes/sensors/r200.dae" />
      </geometry>
    </visual>
    <collision>
      <origin xyz="0.003 0.065 0.007" rpy="0 0 0"/>
      <geometry>
        <box size="0.012 0.132 0.020"/>
      </geometry>
    </collision>

    <!-- This inertial field needs doesn't contain reliable data!! -->
<!--   <inertial>
      <mass value="0.564" />
      <origin xyz="0 0 0" />
      <inertia ixx="0.003881243" ixy="0.0" ixz="0.0"
               iyy="0.000498940" iyz="0.0"
               izz="0.003879257" />
    </inertial>-->
  </link>

  <joint name="camera_rgb_joint" type="fixed">
    <origin xyz="${r200_cam_rgb_px} ${r200_cam_rgb_py} ${r200_cam_rgb_pz}" rpy="0 0 0"/>
    <parent link="camera_link"/>
    <child link="camera_rgb_frame"/>
  </joint>
  <link name="camera_rgb_frame"/>

  <joint name="camera_rgb_optical_joint" type="fixed">
    <origin xyz="0 0 0" rpy="-1.57 0 -1.57"/>
    <parent link="camera_rgb_frame"/>
    <child link="camera_rgb_optical_frame"/>
  </joint>
  <link name="camera_rgb_optical_frame"/>

  <joint name="camera_depth_joint" type="fixed">
    <origin xyz="${r200_cam_rgb_px} ${r200_cam_rgb_py + r200_cam_depth_offset} ${r200_cam_rgb_pz}" rpy="0 0 0"/>
    <parent link="camera_link"/>
    <child link="camera_depth_frame"/>
  </joint>
  <link name="camera_depth_frame"/>

  <joint name="camera_depth_optical_joint" type="fixed">
    <origin xyz="0 0 0" rpy="-1.57 0 -1.57"/>
    <parent link="camera_depth_frame"/>
    <child link="camera_depth_optical_frame"/>
  </joint>
  <link name="camera_depth_optical_frame"/>


  <xacro:include filename="$(find wpr_simulation)/urdf/VLP-16.urdf.xacro"/>
  <xacro:VLP-16 parent="base_link" name="velodyne" topic="/velodyne_points" hz="10" samples="440" gpu="false">
    <origin xyz="0 0 0.4" rpy="0 0 0" />
  </xacro:VLP-16>

</robot>

运行自己组合的（如需组建自己的仿真环境可自行搜索）

下一节将在仿真环境中进行3D建图