移动机器人gazebo仿真(1)—导入激光雷达

参考文章：

http://blog.csdn.net/qq_36355662/article/details/61413860

http://blog.csdn.net/qq_36355662/article/details/61616513

http://www.ncnynl.com/archives/201610/914.html

http://www.yaoguangkeji.com/a_d3O8YpEk.html

http://blog.csdn.net/changer_sun/article/details/79264388#t2

http://blog.csdn.net/zyh821351004/article/details/48846179

1、准备源码、模型

首先建立工作空间并下载源码

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  

turtlebot_interactions

下载模型并放在～/.gazebo/models 路径下

cd ~/.gazebo/models
wget -q -R *index.html*,*.tar.gz --no-parent -r -x -nH http://models.gazebosim.org/hokuyo/

也可下载全部模型，放到隐藏目录.gazebo下（ctrl+H显示隐藏目录）

扫描二维码关注公众号，回复： 892307 查看本文章

地址：

2、修改turtlebot_world.launch

修改 STACKS 申明 ：

<arg name="stacks"     value="$(optenv TURTLEBOT_STACKS hexagons)"/>

修改TURTLEBOT_3D_SENSOR：

<arg name="3d_sensor" value="$(optenv TURTLEBOT_3D_SENSOR hokuyo)"/>

修改后如下：

<launch>
  <arg name="gui"        default="true"/>
  <arg name="world_file" default="$(env TURTLEBOT_GAZEBO_WORLD_FILE)"/>
  <arg name="base"       value="$(optenv TURTLEBOT_BASE kobuki)"/> <!-- create, roomba -->
  <arg name="battery"    value="$(optenv TURTLEBOT_BATTERY /proc/acpi/battery/BAT0)"/>  <!-- /proc/acpi/battery/BAT0 -->
  <arg name="stacks"     value="$(optenv TURTLEBOT_STACKS hexagons)"/>  <!-- circles, hexagons -->
  <!--arg name="stacks"     value="$(optenv TURTLEBOT_STACKS nostack)"/-->

  <!--arg name="3d_sensor"  value="$(optenv TURTLEBOT_3D_SENSOR kinect)"/-->  <!-- kinect, asus_xtion_pro -->
  <!--arg name="3d_sensor"  value="$(optenv TURTLEBOT_3D_SENSOR rplidar)"/--> <!-- 修改   -->
  <arg name="3d_sensor" value="$(optenv TURTLEBOT_3D_SENSOR hokuyo)"/>

  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="use_sim_time" value="true"/>
    <arg name="debug" value="false"/>
    <arg name="gui" value="$(arg gui)" />
    <arg name="world_name" value="$(arg world_file)"/>
  </include>

  <include file="$(find turtlebot_gazebo)/launch/includes/$(arg base).launch.xml">
    <arg name="base" value="$(arg base)"/>
    <arg name="stacks" value="$(arg stacks)"/>
    <!--arg name="stacks" default="nostack"/-->
    <arg name="3d_sensor" value="$(arg 3d_sensor)"/>
    <!--arg name="3d_sensor" default="rplidar"/-->
  </include>

  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
    <param name="publish_frequency" type="double" value="30.0" />
  </node>

  <!-- Fake laser 假的激光 -->
  <node pkg="nodelet" type="nodelet" name="laserscan_nodelet_manager" args="manager"/>
  <!--node pkg="nodelet" type="nodelet" name="depthimage_to_laserscan"
        args="load depthimage_to_laserscan/DepthImageToLaserScanNodelet laserscan_nodelet_manager">
    <param name="scan_height" value="10"/>
    <param name="output_frame_id" value="/camera_depth_frame"/>
    <param name="range_min" value="0.45"/>
    <remap from="image" to="/camera/depth/image_raw"/>
    <remap from="scan" to="/scan">
  </node此处有修改！！！！-->
</launch>

3、分析turtlebot_world.launch

turtlebot_world.launch 功能：

1、找到 gazebo_ros包下的empty_world.launch文件 ，作为gazebo的地图，该文件可以自行替换为自建地图

2、找到 turtlebot_gazebo包下的kobuki.launch.xml文件

3、运行

<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">

4、添加 假的激光

4、修改kobuki.launch.xml文件

找到turtlebot_gazebo包

roscd turtlebot_gazebo/

找到xml文件，地址：

/home/jk/gmapping_ws/src/turtlebot_simulator/turtlebot_gazebo/launch/includes

修改xml文件如下：

<launch>
  <arg name="base"/>
  <arg name="stacks"/>
  <arg name="3d_sensor"/>
  
<arg name="urdf_file" default="$(find xacro)/xacro.py '$(find turtlebot_description)/robots/$(arg base)_$(arg stacks)_$(arg 3d_sensor).urdf.xacro'" />
  <param name="robot_description" command="$(arg urdf_file)"/>

  <!-- Gazebo model spawner -->
  <node name="spawn_turtlebot_model" pkg="gazebo_ros" type="spawn_model"
        args="$(optenv ROBOT_INITIAL_POSE) -unpause -urdf -param robot_description -model mobile_base"/>

  <!-- Velocity muxer -->
  <node pkg="nodelet" type="nodelet" name="mobile_base_nodelet_manager" args="manager"/>
  <node pkg="nodelet" type="nodelet" name="cmd_vel_mux"
        args="load yocs_cmd_vel_mux/CmdVelMuxNodelet mobile_base_nodelet_manager">
    <param name="yaml_cfg_file" value="$(find turtlebot_bringup)/param/mux.yaml"/>
    <remap from="cmd_vel_mux/output" to="mobile_base/commands/velocity"/>
  </node>

  <!-- Bumper/cliff to pointcloud (not working, as it needs sensors/core messages) -->
  <include file="$(find turtlebot_bringup)/launch/includes/kobuki/bumper2pc.launch.xml"/>
</launch>

3.修改kobuki_hexagons_hokuyo.urdf.xacro

新建文件：

roscd turtlebot_description/robots/
sudo vi kobuki_hexagons_hokuyo.urdf.xacro

内容：

<?xml version="1.0"?>
<!--
    - Base      : kobuki
    - Stacks    : hexagons
    - 3d Sensor : kinect+hokuyo
-->
<robot name="turtlebot" xmlns:xacro="http://ros.org/wiki/xacro">

  <xacro:include filename="$(find turtlebot_description)/urdf/turtlebot_library.urdf.xacro" />
  <xacro:include filename="$(find kobuki_description)/urdf/kobuki.urdf.xacro" />
  <xacro:include filename="$(find turtlebot_description)/urdf/stacks/hexagons.urdf.xacro"/>
  <xacro:include filename="$(find turtlebot_description)/urdf/sensors/kinect.urdf.xacro"/>
  <xacro:include filename="$(find turtlebot_description)/urdf/sensors/hokuyo.urdf.xacro"/>

  <kobuki/>
  <stack_hexagons parent="base_link"/>
  <sensor_kinect  parent="base_link"/>
  <sensor_hokuyo parent="base_link"/>
</robot>

注意这两处有修改（删除了原先的turtlebot_common_library,加入了以下内容）

<xacro:include filename="$(find turtlebot_description)/urdf/turtlebot_library.urdf.xacro" />
<!--这里从turtlebot_common_library改到了turtlebot_library-->
<xacro:include filename="$(find turtlebot_description)/urdf/sensors/hokuyo.urdf.xacro"/>
<!--这个是加上的新的-->

4.修改turtlebot_library.urdf.xacro

找到路径：

roscd turtlebot_description/urdf/
gedit 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"/>
  <xacro:include filename="$(find turtlebot_description)/urdf/sensors/hokuyo.urdf.xacro"/>
</robot>

hokuyo.urdf.xacro是新加入的传感器urdf文件，因此需要新建;

5.添加hokuyo的.urdf.xacro文件

roscd turtlebot_description/urdf/sensors/
gedit hokuyo.urdf.xacro

编辑文件内容如下：

<?xml version="1.0"?>
    <!-- script_version=1.1 -->
    <robot name="sensor_hokuyo" 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_hokuyo" params="parent">
        <joint name="laser" type="fixed">
          <origin xyz="0.10 0 0.435" rpy="0 0.0 0.0" />
          <parent link="base_link" />
            <child link="hokuyo_link" />
        </joint>

        <!-- Hokuyo Laser -->
       <link name="hokuyo_link">
         <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>
        <mesh filename="package://turtlebot_description/meshes/sensors/hokuyo.dae"/>
         </geometry>
       </visual>

       <inertial>
           <mass value="1e-5" />
         <origin xyz="0 0 0" rpy="0 0 0"/>
        <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
        </inertial>
       </link>

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

这里有一步：

<mesh filename="package://turtlebot_description/meshes/sensors/hokuyo.dae"/>

需要将路径：/home/jk/.gazebo/models/hokuyo/meshes  中的hokuyo_convex.stl 和 hokuyo.dae复制到

 /home/jk/gmapping_ws/src/turtlebot/turtlebot_description/meshes/sensors    下

6.配置turtlebot_gazebo.urdf.xacro添加laser

roscd turtlebot_description/urdf/
gedit 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-->  
  
  <xacro:macro name="turtlebot_sim_2dsensor">
 <gazebo reference="hokuyo_link">
        <sensor type="ray" name="head_hokuyo_sensor">
          <pose>0 0 0 0 0 0</pose>
          <visualize>false</visualize>
          <update_rate>40</update_rate>
          <ray>
            <scan>
              <horizontal>
                <samples>720</samples>
                <resolution>1</resolution>
                <min_angle>-1.570796</min_angle>
                <max_angle>1.570796</max_angle>
              </horizontal>
</scan>
            <range>
              <min>0.10</min>
              <max>30.0</max>
              <resolution>0.01</resolution>
            </range>
            <noise>
              <type>gaussian</type>

               <mean>0.0</mean>
              <stddev>0.01</stddev>
            </noise>
          </ray>
            <plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_laser.so">
            <!--topicName>/turtlebot/laser/scan</topicName-->
            <topicName>scan</topicName>
            <frameName>hokuyo_link</frameName>
        </plugin>
        </sensor>
      </gazebo>
  </xacro:macro>
</robot>  

此处注意hokuyo_link部件名称和前边的hokuyo_link对应，才能仿真处实际传感器数据。此处数据发布在<topicName>/turtlebot/laser/scan</topicName>中的/turtlebot/laser/scan主题上。

7、修改环境变量

sudo gedit .bashrc

#在最后添加以下内容
#For gazebo 
export  TURTLEBOT_BASE=kobuki
export  TURTLEBOT_STACKS=hexagons
export  TURTLEBOT_3D_SENSOR=hokuyo

之后运行，正常，激光有数据，话题/scan正常

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