移动机器人gazebo仿真(2)—导入kinetic和hokuyo激光雷达

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

补充：首先先试试看能否运行以上命令，不能的话就安装相应的包吧。

参考文章：

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

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

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

上一篇博客建图时打开rviz会造成gazeon进程中断，因此干脆另外使用激光传感器hokuyo与kinect共存

为方便日后使用，将所有/opt/ros/kinetic/share下有修改的代码都另建一份，放在工作空间的pkg下。

文末尾上传了代码，实现gazeon下的建图仿真

有修改的软件包如下：

即上图显示的程序都需要修改；下面贴出代码：

file： my_world_hk.launch

<launch>
  <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="gui" default="true"/>
  <arg name="stacks"    value="$(optenv TURTLEBOT_STACKS hexagons)"/>  <!-- circles, hexagons --> 
  <!--arg name="3d_sensor" value="$(optenv TURTLEBOT_3D_SENSOR kinect)"/-->  <!-- kinect, asus_xtion_pro --> 

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


  <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"-->
 <include file="/home/jk/ros_sim/src/robot_hokuyo_kinect/launch/include/$(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>
  
  <!-- 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>

file:kobuki.launch.xml

<launch>
  <arg name="base"/>
  <arg name="stacks"/>
  <arg name="3d_sensor"/>
  
  <arg name="urdf_file" default="$(find xacro)/xacro.py '/home/jk/ros_sim/src/robot_hokuyo_kinect/launch/include/robot/$(arg base)_$(arg stacks)_$(arg 3d_sensor).urdf.xacro'" />
  <param name="robot_description" command="$(arg urdf_file)" />
  
 <!--<arg name="urdf_file" default="$(find xacro)/xacro.py '$(find turtlebot_description)/robots/$(arg base)_$(arg stacks)_$(arg 3d_sensor).urdf.xacro'"/>-->


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

file:hokuyo.dae (此文件实际需要下载model，下载方式如下)

以下是要放到robot文件夹下的代码

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

file: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/astra.urdf.xacro"/>
  <xacro:include filename="$(find turtlebot_description)/urdf/sensors/r200.urdf.xacro"/>
  <!--修改-->
    <xacro:include filename="/home/jk/ros_sim/src/robot_hokuyo_kinect/launch/include/robot/hokuyo.urdf.xacro"/>  
</robot>

file: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>B8G8R8</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>  
<span style="color:#FF0000;">  
<span style="color:#000000;">
 <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>  
              <!-- Noise parameters based on published spec for Hokuyo laser  
                   achieving "+-30mm" accuracy at range < 10m.  A mean of 0.0m and  
                   stddev of 0.01m will put 99.7% of samples within 0.03m of the true  
                   reading. -->  
               <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>
            <frameName>hokuyo_link</frameName>  
          </plugin>  
        </sensor>  
      </gazebo>  
  </xacro:macro></span></span>  
 </robot>

file:kobuki_hexagons_KinectHokuyo.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="/home/jk/ros_sim/src/robot_hokuyo_kinect/launch/include/robot/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="/home/jk/ros_sim/src/robot_hokuyo_kinect/launch/include/robot/hokuyo.urdf.xacro"/>
  
  <kobuki/>
  <stack_hexagons parent="base_link"/>
  <sensor_kinect  parent="base_link"/>

  <sensor_hokuyo parent="base_link"/>
</robot>

file:hokuyo.urdf.xacro

下面这条代码需要根据实际路径做具体修改！！！！！！！

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

<?xml version="1.0"?>  
    <!-- script_version=1.1 -->  
    <robot name="sensor_hokuyo" xmlns:xacro="http://ros.org/wiki/xacro">  
      <xacro:include filename="/home/jk/ros_sim/src/robot_hokuyo_kinect/launch/include/robot/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>

以下文件和gmapping有关

file:gmapping_demo.launch （用于建立地图）

<launch>
  <arg name="3d_sensor" default="$(env TURTLEBOT_3D_SENSOR)"/>  <!-- r200, kinect, asus_xtion_pro -->
  <arg name="custom_gmapping_launch_file" default="/home/jk/ros_sim/src/robot_hokuyo_kinect/launch/include/gmapping/$(arg 3d_sensor)_gmapping.launch.xml"/>
  <include file="$(arg custom_gmapping_launch_file)"/>
</launch>

gmapping文件夹下file： KinectHokuyo_gmapping.launch.xml

<launch>
  <!--<arg name="scan_topic"  default="scan" />-->
  <arg name="scan_topic"  default="/turtlebot/laser/scan" />
  <arg name="base_frame"  default="base_footprint"/>
  <arg name="odom_frame"  default="odom"/>

  <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
    <param name="base_frame" value="$(arg base_frame)"/>
    <param name="odom_frame" value="$(arg odom_frame)"/>
    <param name="map_update_interval" value="5.0"/>
    <param name="maxUrange" value="6.0"/>
    <param name="maxRange" value="8.0"/>
    <param name="sigma" value="0.05"/>
    <param name="kernelSize" value="1"/>
    <param name="lstep" value="0.05"/>
    <param name="astep" value="0.05"/>
    <param name="iterations" value="5"/>
    <param name="lsigma" value="0.075"/>
    <param name="ogain" value="3.0"/>
    <param name="lskip" value="0"/>
    <param name="minimumScore" value="200"/>
    <param name="srr" value="0.01"/>
    <param name="srt" value="0.02"/>
    <param name="str" value="0.01"/>
    <param name="stt" value="0.02"/>
    <param name="linearUpdate" value="0.5"/>
    <param name="angularUpdate" value="0.436"/>
    <param name="temporalUpdate" value="-1.0"/>
    <param name="resampleThreshold" value="0.5"/>
    <param name="particles" value="80"/>
  <!--
    <param name="xmin" value="-50.0"/>
    <param name="ymin" value="-50.0"/>
    <param name="xmax" value="50.0"/>
    <param name="ymax" value="50.0"/>
  make the starting size small for the benefit of the Android client's memory...
  -->
    <param name="xmin" value="-1.0"/>
    <param name="ymin" value="-1.0"/>
    <param name="xmax" value="1.0"/>
    <param name="ymax" value="1.0"/>

    <param name="delta" value="0.05"/>
    <param name="llsamplerange" value="0.01"/>
    <param name="llsamplestep" value="0.01"/>
    <param name="lasamplerange" value="0.005"/>
    <param name="lasamplestep" value="0.005"/>
    <remap from="scan" to="$(arg scan_topic)"/>
  </node>
</launch>

注意在~/.bashrc中添加以下内容：

source /home/jk/ros_sim/devel/setup.bash
export  TURTLEBOT_BASE=kobuki  
export  TURTLEBOT_STACKS=hexagons
export  TURTLEBOT_3D_SENSOR=KinectHokuyo

分别在终端运行：

roslaunch robot_hokuyo_kinect my_world_hk.launch 
roslaunch robot_hokuyo_kinect gmapping_demo.launch 
roslaunch turtlebot_rviz_launchers view_navigation.launch  
roslaunch turtlebot_teleop keyboard_teleop.launch

运行 rqt_graph看到

gazon和rviz效果图：

代码压缩包上传至博客： ros_sim

移动机器人gazebo仿真(2)—导入kinetic和hokuyo激光雷达

猜你喜欢