2D SLAM : gmapping

1. 準備的原始碼。

   可以建一個新的ros工作空間 gampping_ws,在github下面下載好相應的原始碼。注意更新下gazebo下的model,不然要線上下載（需要goole），所以

給你個離線的包： 連結: http://pan.baidu.com/s/1bnE0mOR 密碼: 9mft     （gazebo_models.zip）

將其下載好解壓替換你安裝的gazebo下的models。 在你home ～ 路徑下。顯示隱藏檔案（ctrl + H ），找到～/.gazebo/models   

mkdir -p ~/gmapping_ws/src
cd ~/gmapping/src
 

git clone https://github.com/robopeak/rplidar_ros.git
git clone https://github.com/turtlebot/turtlebot_simulator.git
git clone https://github.com/turtlebot/turtlebot_apps.git
git clone https://github.com/turtlebot/turtlebot.git

2. 原始碼進行相應的gazebo + gampping    （利用kinect三維點雲模擬2D laser）

 先啟動gazeb， 相當於硬體啟動，感測器都啟動起來了。 /scan   /dom 有資料了。

啟動gmapping ， 利用採集到的感測器資料進行製圖。   /map 有資料

執行rviz 顯示相應的製圖資訊。

最後執行鍵盤控制機器人運動製圖。   

roslaunch turtlebot_gazebo turtlebot_world.launch
roslaunch turtlebot_gazebo gmapping_demo.launch 
roslaunch turtlebot_rviz_launchers view_navigation.launch 
roslaunch turtlebot_teleop keyboard_teleop.launch 

下圖是實際執行的效果圖：

3. 替換感測器介面  (kinct -> rplidar )

  分析： 模擬也就是利用模擬環境下與模擬環境下的感測器感受虛擬的世界，將其感受到的資訊釋出出來。。（底層採集： 具有依賴性  ）

      上層算法制圖依賴底層的資料介面。（滿足相應的資料介面就可以使用。fake相應的sensor data 也可以，堆底層具體依賴不強）

==》 模擬更換感測器就需要更改模型urdf檔案，同時在gazebo裡面加入相應的感測器外掛去感受相應的模擬環境，並以topic的形式釋出出來。

      從上面的分析加上上面原始碼launch檔案看===》只需關注  turtlebot_world.launch  檔案就可以。

 3.1. 看 roslaunchturtlebot_gazebo turtlebot_world.launch  檔案。

           與感測器模型相關的 語句是： base --- 3d_sensor ===》要換成rplidar

                  <includefile="$(find turtlebot_gazebo)/launch/includes/$(argbase).launch.xml">

 3.2.  看kobuki.launch.xml 檔案。= > 關注urdf 檔案 ==> 路徑在/turtlebot/turtlebot_description/robots 

        <argname="urdf_file" default="$(find xacro)/xacro.py'$(find turtlebot_description)/robots/$(arg base)_$(arg stacks)_$(arg3d_sensor).urdf.xacro'" />

 3.3. 在/turtlebot/turtlebot_description/robots下建立相應的urdf檔案  

        仿照kobuki_hexagons_kinect.urdf.xacro檔案建立

kobuki_nostack_rplidar.urdf.xacro

kobuki_hexagons_rplidar.urdf.xacro

<sensor_rplidar parent="base_link"/>

<xacro:includefilename="$(findturtlebot_description)/urdf/turtlebot_library.urdf.xacro" />

3.4.  進入檔案$(findturtlebot_description)/urdf/turtlebot_library.urdf.xacro新增rplidar的urdf檔案

<xacro:includefilename="$(findturtlebot_description)/urdf/sensors/rplidar.urdf.xacro"/>

3.5.  在路徑$(findturtlebot_description)/urdf/sensors/下建立rplidar.urdf.xacro urdf檔案

配置位置資訊。laser相對robot的位置 座標系的資訊

<?xml version="1.0"?>
<!-- script_version=1.1 -->
<robot name="sensor_rplidar" xmlns:xacro="http://ros.org/wiki/xacro">
  <xacro:include filename="$(find turtlebot_description)/urdf/turtlebot_gazebo.urdf.xacro"/>
  <xacro:include filename="$(find turtlebot_description)/urdf/turtlebot_properties.urdf.xacro"/>

  <!-- RPLidar 2D LIDAR -->
  <xacro:macro name="sensor_rplidar" params="parent">
    <joint name="laser" type="fixed">
      <origin xyz="0.15 0.0 0.15" rpy="0 0.0 0.0" />
      <parent link="base_link" />
      <child link="base_laser_link" />
    </joint>

    <link name="base_laser_link">
      <visual>
        <geometry>
          <box size="0.05 0.05 0.05" />
        </geometry>
        <material name="Red" />
      </visual>
      <inertial>
        <mass value="0.000001" />
        <origin xyz="0 0 0" />
        <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
          iyy="0.0001" iyz="0.0"
          izz="0.0001" />
      </inertial>
    </link>

    <!-- Set up laser gazebo details -->
    <rplidar_laser />
  </xacro:macro>
</robot>

3.6.  在<xacro:include filename="$(findturtlebot_description)/urdf/turtlebot_gazebo.urdf.xacro"/>檔案下新增配置laser在gazebo下的屬性值外掛配置

<!-- RPLidar LIDAR for simulation -->
  <xacro:macro name="rplidar_laser">
    <gazebo reference="base_laser_link">
      <sensor type="ray" name="laser">
        <pose>0 0 0 0 0 0</pose>
        <visualize>false</visualize>
        <update_rate>5.5</update_rate>
        <ray>
          <scan>
            <horizontal>
              <samples>360</samples>
              <resolution>1</resolution>
              <min_angle>-3.1415926</min_angle>
              <max_angle>3.1415926</max_angle>
            </horizontal>
          </scan>
          <range>
            <min>0.10</min>
            <max>6.0</max>
            <resolution>0.01</resolution>
          </range>
          <noise>
            <type>Gaussian</type>
            <mean>0.0</mean>
            <stddev>0.01</stddev>
          </noise>
        </ray>
        <plugin name="rplidar_node" filename="libgazebo_ros_laser.so">
          <topicName>/scan</topicName>
          <frameName>base_laser_link</frameName>
        </plugin>
      </sensor>
    </gazebo>
  </xacro:macro>

3.7.  環境變數加入

export TURTLEBOT_BASE=kobuki
export TURTLEBOT_STACKS=nostack
export TURTLEBOT_3D_SENSOR=rplidar

執行一致

roslaunch turtlebot_gazebo turtlebot_world.launch
roslaunch turtlebot_gazebo gmapping_demo.launch 
roslaunch turtlebot_rviz_launchers view_navigation.launch 
roslaunch turtlebot_teleop keyboard_teleop.launch 

4.  涉及更改的檔案

/home/yhzhao/gmapping_ws/src/turtlebot/turtlebot_description/robots/kobuki_nostack_rplidar.urdf.xacro

<?xml version="1.0"?>
<!-- script_version=1.1 -->
<!--
    - Base      : kobuki
    - Stacks    : nostack
    - 3d Sensor : rplidar
-->    
<robot name="turtlebot" xmlns:xacro="http://ros.org/wiki/xacro">

  <xacro:include filename="$(find turtlebot_description)/urdf/turtlebot_library.urdf.xacro" />
  
  <kobuki/>
  <!--<stack_hexagons parent="base_link"/>-->
  <sensor_rplidar  parent="base_link"/>
</robot>

 /home/yhzhao/gmapping_ws/src/turtlebot/turtlebot_description/urdf/turtlebot_library.urdf.xacro

<?xml version="1.0"?>
<!--
  The complete turtlebot library of xacros for easy reference
 -->
<robot xmlns:xacro="http://ros.org/wiki/xacro">
  <!-- General -->
  <xacro:include filename="$(find turtlebot_description)/urdf/common_properties.urdf.xacro"/>
  <xacro:include filename="$(find turtlebot_description)/urdf/turtlebot_properties.urdf.xacro"/>
  <!-- Bases -->
  <xacro:include filename="$(find create_description)/urdf/create.urdf.xacro"/>
  <xacro:include filename="$(find kobuki_description)/urdf/kobuki.urdf.xacro" />
  <!-- Stacks -->
  <xacro:include filename="$(find turtlebot_description)/urdf/stacks/circles.urdf.xacro"/>
  <xacro:include filename="$(find turtlebot_description)/urdf/stacks/hexagons.urdf.xacro"/>
  <!-- 3D Sensors -->
  <xacro:include filename="$(find turtlebot_description)/urdf/sensors/kinect.urdf.xacro"/>
  <xacro:include filename="$(find turtlebot_description)/urdf/sensors/asus_xtion_pro.urdf.xacro"/>
  <xacro:include filename="$(find turtlebot_description)/urdf/sensors/asus_xtion_pro_offset.urdf.xacro"/>
<xacro:include filename="$(find turtlebot_description)/urdf/sensors/rplidar.urdf.xacro"/>

</robot>

/home/yhzhao/gmapping_ws/src/turtlebot/turtlebot_description/urdf/sensors/rplidar.urdf.xacro

<?xml version="1.0"?>
<!-- script_version=1.1 -->
<robot name="sensor_rplidar" xmlns:xacro="http://ros.org/wiki/xacro">
  <xacro:include filename="$(find turtlebot_description)/urdf/turtlebot_gazebo.urdf.xacro"/>
  <xacro:include filename="$(find turtlebot_description)/urdf/turtlebot_properties.urdf.xacro"/>

  <!-- RPLidar 2D LIDAR -->
  <xacro:macro name="sensor_rplidar" params="parent">
    <joint name="laser" type="fixed">
      <origin xyz="0.15 0.0 0.15" rpy="0 0.0 0.0" />
      <parent link="base_link" />
      <child link="base_laser_link" />
    </joint>

    <link name="base_laser_link">
      <visual>
        <geometry>
          <box size="0.05 0.05 0.05" />
        </geometry>
        <material name="Red" />
      </visual>
      <inertial>
        <mass value="0.000001" />
        <origin xyz="0 0 0" />
        <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
          iyy="0.0001" iyz="0.0"
          izz="0.0001" />
      </inertial>
    </link>

    <!-- Set up laser gazebo details -->
    <rplidar_laser />
  </xacro:macro>
</robot>

/home/yhzhao/gmapping_ws/src/turtlebot/turtlebot_description/urdf/turtlebot_gazebo.urdf.xacro

<?xml version="1.0"?>
<robot name="turtlebot_gazebo" xmlns:xacro="http://ros.org/wiki/xacro">
  <!-- Microsoft Kinect / ASUS Xtion PRO Live for simulation -->
  <xacro:macro name="turtlebot_sim_3dsensor">
    <gazebo reference="camera_link">  
      <sensor type="depth" name="camera">
        <always_on>true</always_on>
        <update_rate>20.0</update_rate>
        <camera>
          <horizontal_fov>${60.0*M_PI/180.0}</horizontal_fov>
          <image>
            <format>R8G8B8</format>
            <width>640</width>
            <height>480</height>
          </image>
          <clip>
            <near>0.05</near>
            <far>8.0</far>
          </clip>
        </camera>
        <plugin name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so">
          <cameraName>camera</cameraName>
          <alwaysOn>true</alwaysOn>
          <updateRate>10</updateRate>
          <imageTopicName>rgb/image_raw</imageTopicName>
          <depthImageTopicName>depth/image_raw</depthImageTopicName>
          <pointCloudTopicName>depth/points</pointCloudTopicName>
          <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
          <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
          <frameName>camera_depth_optical_frame</frameName>
          <baseline>0.1</baseline>
          <distortion_k1>0.0</distortion_k1>
          <distortion_k2>0.0</distortion_k2>
          <distortion_k3>0.0</distortion_k3>
          <distortion_t1>0.0</distortion_t1>
          <distortion_t2>0.0</distortion_t2>
          <pointCloudCutoff>0.4</pointCloudCutoff>
        </plugin>
      </sensor>
    </gazebo>
  </xacro:macro>


<!-- RPLidar LIDAR for simulation -->
  <xacro:macro name="rplidar_laser">
    <gazebo reference="base_laser_link">
      <sensor type="ray" name="laser">
        <pose>0 0 0 0 0 0</pose>
        <visualize>false</visualize>
        <update_rate>5.5</update_rate>
        <ray>
          <scan>
            <horizontal>
              <samples>360</samples>
              <resolution>1</resolution>
              <min_angle>-3.1415926</min_angle>
              <max_angle>3.1415926</max_angle>
            </horizontal>
          </scan>
          <range>
            <min>0.10</min>
            <max>6.0</max>
            <resolution>0.01</resolution>
          </range>
          <noise>
            <type>Gaussian</type>
            <mean>0.0</mean>
            <stddev>0.01</stddev>
          </noise>
        </ray>
        <plugin name="rplidar_node" filename="libgazebo_ros_laser.so">
          <topicName>/scan</topicName>
          <frameName>base_laser_link</frameName>
        </plugin>
      </sensor>
    </gazebo>
  </xacro:macro>



</robot>

參考：  

擴充套件 (模擬器) ： vrep_ros_bridge  http://wiki.ros.org/vrep_ros_bridge            http://www.zhihu.com/question/23309094

================