Gmapping+Laser_scan_matcher仿真

在gazebo上做仿真时运动仿真插件可以提供里程计坐标系到机器人底座坐标系的转换（odom→base_footprint），但是做实物调试时通过底盘的编码器做出来的增量式里程计效果可能不太理想，所以可以通过雷达扫描匹配功能包laser_scan_matcher提供里程计信息（odom→base_footprint），这里会把gazebo运动仿真插件提供的 （odom→base_footprint） 转换屏蔽，直接由功能包 laser_scan_matcher 提供。

１、首先安装两个 laser_scan_matcher 相关的功能包 laser_scan_matcher 和 csm 。

安装laser_scan_matcher：

$sudo apt-get install ros-kinetic-laser-scan-matcher

安装 csm :

$sudo apt-get install ros-kinetic-csm

２、创建launch文件 laser_scan_matcger.launch 运行 laser_scan_matcher ，把 fixed_frame 设置为 odom ，把 base_frame 设置为 base_footprint 。

<?xml version="1.0"?>
<launch>

    <node name="laser_scan_matcher_node" pkg="laser_scan_matcher" type="laser_scan_matcher_node"  
          output="screen" >
        <param name="fixed_frame" value = "odom"/>
        <param name="base_frame" value = "base_footprint"/>
        <param name="use_imu" value = "true"/>
        <param name="max_iterations" value="10"/>
    </node>

</launch>

３、把 gazebo 的运动仿真插件提供的 tf 变换屏蔽，把 mbot_base_gazebo.xacro文件中的<publishTf>0</publishTf>和<broadcastTF>0</broadcastTF>设置为0。

<gazebo>
            <plugin name="differential_drive_controller" 
                    filename="libgazebo_ros_diff_drive.so">
                <rosDebugLevel>Debug</rosDebugLevel>
                <publishWheelTF>true</publishWheelTF>
                <robotNamespace>/</robotNamespace>
                <publishTf>0</publishTf>
                <publishWheelJointState>true</publishWheelJointState>
                <alwaysOn>true</alwaysOn>
                <updateRate>100.0</updateRate>
                <legacyMode>true</legacyMode>
                <leftJoint>left_wheel_joint</leftJoint>
                <rightJoint>right_wheel_joint</rightJoint>
                <wheelSeparation>${wheel_joint_y*2}</wheelSeparation>
                <wheelDiameter>${2*wheel_radius}</wheelDiameter>
                <broadcastTF>0</broadcastTF>
                <wheelTorque>30</wheelTorque>
                <wheelAcceleration>1.8</wheelAcceleration>
                <commandTopic>cmd_vel</commandTopic>
                <odometryFrame>odom</odometryFrame> 
                <odometryTopic>odom</odometryTopic> 
                <robotBaseFrame>base_footprint</robotBaseFrame>
            </plugin>
</gazebo> 

４、创建 gmapping launch文件gmapping_demo.launch同时运行 gmapping 和 rviz

<?xml version="1.0"?>
<launch>
    <arg name="scan_topic" default="scan" />

    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen" clear_params="true">
        <param name="odom_frame" value="odom"/>
        <param name="map_update_interval" value="5.0"/>
        <!-- Set maxUrange < actual maximum range of the Laser -->
        <param name="maxRange" value="5.0"/>
        <param name="maxUrange" value="4.5"/>
        <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="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="-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>
　　
<!-- 启动rviz -->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find robot_navigation)/rviz/gmapping.rviz"/>

</launch>

5、打开仿真环境

$roslaunch robot_gazebo mbot_laser_nav_gazebo.launch

6、运行 laser_scan_matcher 

$roslaunch robot_navigation laser_scan_matcher.launch

７、运行 gmapping_demo.launch 

$roslaunch robot_navigation gmapping_demo.launch

８、查看 tf 树检查 laser_scan_matcher 是否提供 odom→base_footprint 的转换。