ROS实现SLAM

由于我是在URDF导出来时就已经有laser这个关节，我就只要在后面加上插件让它变成传感器就好，如果大家想在后面自行添加laser模型，可以参考一下xacro优化6.7.2 雷达信息仿真以及显示 · Autolabor-ROS机器人入门课程《ROS理论与实践》零基础教程

首先我们要先在URDF装入laser的插件

  <!-- add laser simulation plugines adaptored for rplidar A1M8 -->
  <gazebo reference="laser_Link">
    <sensor type="ray" name="Top_laser_sensor">
      <visualize>false</visualize>   <!-- visual or unvisual laser ray :true or false-->
      <update_rate>40</update_rate> <!-- default is : 40. change the update_rate to 5 hz for your rf2o_laser_odometry mapping -->
      <ray>
        <scan>
          <horizontal>
            <samples>720</samples>
            <resolution>1</resolution>
            <min_angle>-3.1415926</min_angle>
            <max_angle>3.1415926</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.4</min>
          <max>30.0</max>
          <resolution>0.06</resolution>
         </range>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin name="gazebo_ros_head_laser_controller" filename="libgazebo_ros_laser.so">
        <topicName>/scan</topicName>
        <frameName>laser_Link</frameName>
      </plugin>
    </sensor>
  </gazebo>

然后我们安装ros已有的slam包

sudo apt install ros-<ROS版本>-gmapping

之后就能编写自己的slam navigation文件

<launch>
<param name="use_sim_time" value="true"/>
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
      <remap from="scan" to="scan"/>
      <param name="base_frame" value="base_footprint"/>
      <param name="odom_frame" value="odom"/> 
      <param name="map_update_interval" value="5.0"/>
      <param name="maxUrange" value="16.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="srr" value="0.1"/>
      <param name="srt" value="0.2"/>
      <param name="str" value="0.1"/>
      <param name="stt" value="0.2"/>
      <param name="linearUpdate" value="1.0"/>
      <param name="angularUpdate" value="0.5"/>
      <param name="temporalUpdate" value="3.0"/>
      <param name="resampleThreshold" value="0.5"/>
      <param name="particles" value="30"/>
      <param name="xmin" value="-50.0"/>
      <param name="ymin" value="-50.0"/>
      <param name="xmax" value="50.0"/>
      <param name="ymax" value="50.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"/>
    </node>

       <node pkg="rviz" type="rviz" name="rviz" />
   

</launch>

其中这些话题是后面一直要用到的，而base_frame是根据自己一开始的定义而来这个不能和之前的URDF冲突

 <remap from="scan" to="scan"/><!-- 雷达话题 -->
<param name="base_frame" value="base_footprint"/><!--底盘坐标系-->
<param name="odom_frame" value="odom"/> <!--里程计坐标系-->

 编写好了slam navigation的launch之后，可以先运行带有机器人和world的launch文件

<launch>

    <!-- 设置launch文件的参数 cooneo_office.world是另外创建的功能包里面的.world文件-->
    <arg name="world_name" value="$(find rob)/worlds/cooneo_office.world"/>
    <arg name="paused" default="false"/>
    <arg name="use_sim_time" default="true"/>
    <arg name="gui" default="true"/>
    <arg name="headless" default="false"/>
    <arg name="debug" default="false"/>

    <!-- 运行gazebo仿真环境 -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="world_name" value="$(arg world_name)" />
        <arg name="debug" value="$(arg debug)" />
        <arg name="gui" value="$(arg gui)" />
        <arg name="paused" value="$(arg paused)"/>
        <arg name="use_sim_time" value="$(arg use_sim_time)"/>
        <arg name="headless" value="$(arg headless)"/>
    </include>

    <!-- 加载机器人模型描述参数 -->
    <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find rob)/urdf/rob.urdf'" /> 

    <!-- 运行joint_state_publisher节点，发布机器人的关节状态  -->
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node> 

    <!-- 运行robot_state_publisher节点，发布tf  -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"  output="screen" >
        <param name="publish_frequency" type="double" value="50.0" />
    </node>

 
    <!-- 在gazebo中加载机器人模型-->
    <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
          args="-urdf -model rob -param robot_description"/> 
<!--
 <node name="mbot_teleop" pkg="rob" type="mbot_teleop.py" respawn="false" output="screen"/>
-->
</launch>
 

待gazebo中模型加载完毕，运行 slam navigation的launch

我们就可以在rviz中，添加已经发布的话题，看到小车可以用laser扫到周围的环境

接下来就用键盘控制结点控制小车跑完地图，获得一张初始的地图

rosrun teleop_twist_keyboard teleop_twist_keyboard.py 

 （注：运行不了这个键盘的，需要手动下载对应ros版本的键盘节点）

跑完初始地图之后，需要地图保存，先下载ros的地图服务包

sudo apt install ros-<ROS版本>-map-server

然后编写地图保存文件

<launch>
    <arg name="filename" value="$(find mycar_nav)/map/nav" />
    <node name="map_save" pkg="map_server" type="map_saver" args="-f $(arg filename)" />
</launch>
 

SLAM建图完毕后，执行该launch文件即可。 

然后就能看到

已经将map保存下来了，并且可以打开查看