SLAM学习笔记——从零搭建完整的gazebo多机仿真SLAM（二）

引言

接着上一课讲的内容，我们使用上一课单机slam构建的地图。这次我们引入多个机器人，并且使用multirobot_map_merge这个包来进行实时融合地图。源文件在第一章的gitee链接中。

编写map_merge文件

这个文件用于启动map_merge服务，接受所有以robot_namespace开头的map文件，并且通过初始位置坐标进行融合，如果不指定初始位置坐标，融合可能依赖特征对其，效果不会太好。

<launch>
  <arg name="robot_name"  default="tb3_0"/>
  
  <arg name="robot_x_pos" default="0.0"/>
  <arg name="robot_y_pos" default="0.0"/>
  <arg name="robot_z_pos" default=" 0.0"/>
  <arg name="robot_yaw"   default=" 0.0"/>
  <arg name="if_first_robot"   default="false"/>

  <group ns="$(arg robot_name)/map_merge">
    <param name="init_pose_x"   value="$(arg robot_x_pos)"/>
    <param name="init_pose_y"   value="$(arg robot_y_pos)"/>
    <param name="init_pose_z"   value="$(arg robot_z_pos)"/>
    <param name="init_pose_yaw" value="$(arg robot_yaw)"  />
  </group>

<group if="$(arg if_first_robot)">
  <node pkg="multirobot_map_merge" type="map_merge" respawn="false" name="map_merge" output="screen">
    <param name="robot_map_topic" value="map"/>
    <!-- 接受所有以tb3开头的map-->
    <param name="robot_namespace" value="tb3"/>
    <param name="merged_map_topic" value="map"/>
    <param name="world_frame" value="map"/>
    <param name="known_init_poses" value="true"/>
    <param name="merging_rate" value="0.5"/>
    <param name="discovery_rate" value="0.05"/>
    <param name="estimation_rate" value="0.5"/>
    <param name="estimation_confidence" value="1.0"/>
  </node>
</group>

  <node pkg="tf" type="static_transform_publisher" name="world_to_$(arg robot_name)_tf_broadcaster"  args="0 0 0 0 0 0 /map /$(arg robot_name)/map 100"/>
</launch>

编写two_robots_slam.launch文件

先后往gazebo环境中放入两台机器人，并且启动地图融合节点

<launch>
    <!--SLAM算法 -->
    <arg name="slam_method" default="gmapping"/>
    <arg name="simulation" default="true"/>

    <group if="$(arg simulation)" >
        <!-- 开启仿真世界 -->
        <include file="$(find gazebo_tutorials)/launch/create_world.launch" />
         <!-- 放置机器人 -->
        <include file="$(find gazebo_tutorials)/launch/place_robot.launch">
            <arg name="robot_x_pos" value="0.0"/>
            <arg name="robot_y_pos" value="0.0"/>
            <arg name="robot_z_pos" value="0.0"/>
            <arg name="robot_yaw"   value="0.0"/>
            <arg name="robot_name" value="tb3_0"/>
        </include>
         <!-- 放置机器人 -->
        <include file="$(find gazebo_tutorials)/launch/place_robot.launch">
            <arg name="robot_x_pos" value="0.5"/>
            <arg name="robot_y_pos" value="0.0"/>
            <arg name="robot_z_pos" value="0.0"/>
            <arg name="robot_yaw"   value="0.0"/>
            <arg name="robot_name" value="tb3_1"/>
        </include>
    </group>
    <!-- 开启SLAM -->
    <include file="$(find gazebo_tutorials)/slam_methods/$(arg slam_method).launch">
        <arg name="ns" value="tb3_0"/>
    </include>
    <include file="$(find gazebo_tutorials)/slam_methods/$(arg slam_method).launch">
        <arg name="ns" value="tb3_1"/>
    </include>

    <!-- 启动地图融合节点并加入1号机器人地图信息 -->
     <include file="$(find gazebo_tutorials)/launch/map_merging.launch">
        <arg name="if_first_robot" value="True"/>
        <arg name="robot_x_pos" value="0.0"/>
        <arg name="robot_y_pos" value="0.0"/>
        <arg name="robot_z_pos" value="0.0"/>
        <arg name="robot_name" value="tb3_0"/>
    </include>
    <!-- 加入2号机器人地图信息 -->
    <include file="$(find gazebo_tutorials)/launch/map_merging.launch">
        <arg name="if_first_robot" value="False"/>
        <arg name="robot_x_pos" value="0.5"/>
        <arg name="robot_y_pos" value="0.0"/>
        <arg name="robot_z_pos" value="0.0"/>
        <arg name="robot_name" value="tb3_1"/>
    </include>   

    <!-- rviz -->
    <include file="$(find gazebo_tutorials)/launch/rviz_two_robots.launch" />
</launch>

RVIZ配置

我们点击add，选择map，然后topic设置为/map，就是合成后的整体地图
然后再加入laserscan扫描点云图和axes机器人坐标

这次我们需要进行的rviz配置就比较多了，所以我们把他保存成配置文件，每次启动launch文件自动运行。
保存方式是就是点击左上角file，save config as，然后编写rviz文件

<launch>
  <!-- rviz -->
    <node pkg="rviz" type="rviz" name="rviz" required="true"
          args="-d $(find gazebo_tutorials)/rviz/two_robots.rviz"/>
</launch>

然后我们只需要直接运行roslaunch gazebo_tutorials two_robots_slam.launch，就可以启动了
最终生成的rqt_graph，结构清晰明了

使用teleop控制和map_saver保存地图

和之前基本一样，记得要更改ROS_NAMESPACE，使用以下命令可以对tb3_0进行操作
ROS_NAMESPACE=tb3_0 rosrun teleop_twist_keyboard teleop_twist_keyboard.py
使用以下命令可以保存地图
rosrun map_server map_saver -f xxx/Map