turtlebot_gazebo仿真添加激光雷达hokuyo_rplidar

ROS中给turtlebot添加传感器：hokuyo，进行gazebo仿真

最初进行仿真的四个步骤：

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

1.编写启动launch文件

由于是apt-get的方式安装的这些包，所以roscd的位置在/opt/ros/kinetic/share这里，不方便的是必须需要sudo的权限才能对文件进行修改

roscd turtlebot_gazebo 
cd launch
/opt/ros/kinetic/share/turtlebot_gazebo/launch

在这里，我不对turtlebot_world.launch进行直接的修改，拷贝一份并改名为explorer.launch,对这个文件进行操作和修改，以后也运行的是这个launch

<launch>
  <arg name="world_file"  default="$(env TURTLEBOT_GAZEBO_WORLD_FILE)"/>

  <arg name="base"      value="$(optenv TURTLEBOT_BASE kobuki)"/> 
  <arg name="battery"   value="$(optenv TURTLEBOT_BATTERY /proc/acpi/battery/BAT0)"/>  
  <arg name="gui" default="true"/>
  <arg name="stacks"    value="$(optenv TURTLEBOT_STACKS hexagons)"/>  
  <arg name="3d_sensor" value="$(optenv TURTLEBOT_3D_SENSOR hokuyo)"/>  

  
  <include file="$(find hector_nist_arena_worlds)/launch/start.launch">
    <arg name="world" value="go2011_arena_changer.world"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="debug" value="false"/>
    <arg name="gui" value="$(arg gui)" />
  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="3d_sensor" value="$(arg 3d_sensor)"/>
  include>

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

  
  <node pkg="nodelet" type="nodelet" name="laserscan_nodelet_manager" args="manager"/>
  
launch>

由上边这一句我们可以知道，调用了了一个配置文件打开

2.查看kobuki.launch.xml

/opt/ros/kinetic/share/turtlebot_gazebo/launch/includes
sudo vi kobuki.launch.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)" />

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

  
  <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>

  
  <include file="$(find turtlebot_bringup)/launch/includes/kobuki/bumper2pc.launch.xml"/>
launch>

这个文件并没有修改，

根据这一行，我们可以知道，启动的 urdf位置和名称

3.修改kobuki_hexagons_hokuyo.urdf.xacro

/opt/ros/kinetic/share/turtlebot_description/robots
sudo vi kobuki_hexagons_hokuyo.urdf.xacro

<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>

这个文件相比于kobuki_hexagons_kinect.urdf.xacro进行了两个地方的修改

<xacro:include filename="$(find turtlebot_description)/urdf/turtlebot_library.urdf.xacro" />

<xacro:include filename="$(find turtlebot_description)/urdf/sensors/hokuyo.urdf.xacro"/>

4.修改turtlebot_library.urdf.xacro

之后我们去修改turtlebot_library.urdf.xacro

/opt/ros/kinetic/share/turtlebot_description/urdf
sudo vi turtlebot_library.urdf.xacro

<robot xmlns:xacro="http://ros.org/wiki/xacro">
  
  <xacro:include filename="$(find turtlebot_description)/urdf/common_properties.urdf.xacro"/>
  <xacro:include filename="$(find turtlebot_description)/urdf/turtlebot_properties.urdf.xacro"/>
  
  <xacro:include filename="$(find create_description)/urdf/create.urdf.xacro"/>
  <xacro:include filename="$(find kobuki_description)/urdf/kobuki.urdf.xacro" />
  
  <xacro:include filename="$(find turtlebot_description)/urdf/stacks/circles.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/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，那么一开始是没有这个urdf的，所以我们需要新建这个文件

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

/opt/ros/kinetic/share/turtlebot_description/urdf/sensors
sudo vi hokuyo.urdf.xacro

    
    <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"/>

      
      <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>

        
       <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>

        
        <turtlebot_sim_2dsensor/>
      xacro:macro>
robot>

我们需要添加这个hokuyo.dae，可以先下载模型包，然后将文件拷贝过来

可以参考gazebo官方教程：http://gazebosim.org/tutorials/?tut=add_laser

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

sun@sun:~/.gazebo/models$ ls
cube_20k  ground_plane  jersey_barrier  pioneer2dx  willowgarage
dumpster  hokuyo        my_robot        sun

sun@sun:~/.gazebo/models$ cd hokuyo/
sun@sun:~/.gazebo/models/hokuyo$ ls
meshes  model-1_2.sdf  model-1_3.sdf  model-1_4.sdf  model.config  model.sdf

sun@sun:~/.gazebo/models/hokuyo$ cd meshes/
sun@sun:~/.gazebo/models/hokuyo/meshes$ ls
hokuyo_convex.stl  hokuyo.dae

查看/opt/ros/kinetic/share/turtlebot_description/meshes/sensors目录，将hokuyo_convex.stl hokuyo.dae两个文件拷贝到该目录即可。

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

/opt/ros/kinetic/share/turtlebot_description/urdf
sudo vi turtlebot_gazebo.urdf.xacro

<robot name="turtlebot_gazebo" xmlns:xacro="http://ros.org/wiki/xacro">
  
  <xacro:macro name="turtlebot_sim_3dsensor">
    <gazebo reference="camera_link">
      <sensor type="depth" name="camera">
        <always_on>truealways_on>
        <update_rate>20.0update_rate>
        <camera>
          <horizontal_fov>${60.0*M_PI/180.0}horizontal_fov>
          <image>
            <format>B8G8R8format>
            <width>640width>
            <height>480height>
          image>
          <clip>
            <near>0.05near>
            <far>8.0far>
          clip>
        camera>
        <plugin name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so">
          <cameraName>cameracameraName>
          <alwaysOn>truealwaysOn>
          <updateRate>10updateRate>
          <imageTopicName>rgb/image_rawimageTopicName>
          <depthImageTopicName>depth/image_rawdepthImageTopicName>
          <pointCloudTopicName>depth/pointspointCloudTopicName>
          <cameraInfoTopicName>rgb/camera_infocameraInfoTopicName>
          <depthImageCameraInfoTopicName>depth/camera_infodepthImageCameraInfoTopicName>
          <frameName>camera_depth_optical_frameframeName>
          <baseline>0.1baseline>
          <distortion_k1>0.0distortion_k1>
          <distortion_k2>0.0distortion_k2>
          <distortion_k3>0.0distortion_k3>
          <distortion_t1>0.0distortion_t1>
          <distortion_t2>0.0distortion_t2>
          <pointCloudCutoff>0.4pointCloudCutoff>
        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 0pose>
          <visualize>falsevisualize>
          <update_rate>40update_rate>
          <ray>
            <scan>
              <horizontal>
                <samples>720samples>
                <resolution>1resolution>
                <min_angle>-1.570796min_angle>
                <max_angle>1.570796max_angle>
              horizontal>
scan>
            <range>
              <min>0.10min>
              <max>30.0max>
              <resolution>0.01resolution>
            range>
            <noise>
              <type>gaussiantype>

               <mean>0.0mean>
              <stddev>0.01stddev>
            noise>
          ray>
            <plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_laser.so">
            
            <topicName>scantopicName>
            <frameName>hokuyo_linkframeName>
        plugin>
        sensor>
      gazebo>
  xacro:macro>
robot>

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

修改环境变量

sudo vi .zshrc #我用的是zsh，如果是bash就修改.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

参考文章：

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