ROS仿真笔记之——多移动机器人SLAM地图融合

之前博客《ROS仿真笔记之——基于gazebo的ROS多机器人仿真》和《ROS实验笔记之——多turtlebot机器人运控实验》已经介绍基于ROS的多移动机器人的仿真与实验了。本博文介绍一下多机器人SLAM。

博文《paper survey之——多机器人协作介绍（Multi-Robot System, MRS）》曾经介绍过多移动机器人的各种应用，其中，一个很重要的应用方向就是多机器人SLAM，通过多个机器人来同时构建SLAM环境地图。注意，本博文中的SLAM是直接将各个机器人构建的地图叠加在一起，而没有采用闭环融合。属于较为简单的多机器人SLAM融合。

首先。在仿真之前确认已下载编译好turtlebot3_simulations，turtlebot3这两个包

仿真之前需要在bashrc中设置好使用的机器人模型是什么，不然的话每次启动都要确认一次：

gedit ~/.bashrc
export TURTLEBOT3_MODEL=waffle

(注意采用. waffle, waffle_pi, burger都可以，按个人喜好选择)

然后按照博文《 ROS仿真笔记之——基于gazebo的ROS多机器人仿真》介绍中那样，启动3个移动机器人的gazebo环境

export TURTLEBOT3_MODEL=waffle
roslaunch turtlebot3_gazebo m_turtlebot3_world.launch

如下图所示

然后分别打开三个新建终端，启动三个机器人的SLAM：

ROS_NAMESPACE=tb3_0 roslaunch turtlebot3_slam turtlebot3_gmapping.launch set_base_frame:=tb3_0/base_footprint set_odom_frame:=tb3_0/odom set_map_frame:=tb3_0/map
ROS_NAMESPACE=tb3_1 roslaunch turtlebot3_slam turtlebot3_gmapping.launch set_base_frame:=tb3_1/base_footprint set_odom_frame:=tb3_1/odom set_map_frame:=tb3_1/map
ROS_NAMESPACE=tb3_2 roslaunch turtlebot3_slam turtlebot3_gmapping.launch set_base_frame:=tb3_2/base_footprint set_odom_frame:=tb3_2/odom set_map_frame:=tb3_2/map

在这里存在一个问题就是，由于之前设计的/home/kwanwaipang/catkin_ws/src/turtlebot3_simulations/turtlebot3_gazebo/launch/m_turtlebot3_world.launch文件并没有把SLAM的数据发布出去（没有添加tf变换）。这样会导致运行了SLAM节点后，出现报错，所以其实为了方便，可以将gazebo的运行命令改为下面

roslaunch simulation multi_turtlebot3.launch

但是本人还是更加偏向于添加SLAM发布块到launch文件中。首先可以打开multi_turtlebot3.launch文件看看其中的关于SLAM发布部分（因为slam它需要选取odom和map所以需要将各组件之间的tf变换关系加入）

<launch>
  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
  <arg name="first_tb3"  default="tb3_0"/>
  <arg name="second_tb3" default="tb3_1"/>
  <arg name="third_tb3"  default="tb3_2"/>

  <arg name="first_tb3_x_pos" default="-7.0"/>
  <arg name="first_tb3_y_pos" default="-1.0"/>
  <arg name="first_tb3_z_pos" default=" 0.0"/>
  <arg name="first_tb3_yaw"   default=" 1.57"/>

  <arg name="second_tb3_x_pos" default=" 7.0"/>
  <arg name="second_tb3_y_pos" default="-1.0"/>
  <arg name="second_tb3_z_pos" default=" 0.0"/>
  <arg name="second_tb3_yaw"   default=" 1.57"/>

  <arg name="third_tb3_x_pos" default=" 0.5"/>
  <arg name="third_tb3_y_pos" default=" 3.0"/>
  <arg name="third_tb3_z_pos" default=" 0.0"/>
  <arg name="third_tb3_yaw"   default=" 0.0"/>

  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find turtlebot3_gazebo)/worlds/turtlebot3_house.world"/>
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="gui" value="true"/>
    <arg name="headless" value="false"/>
    <arg name="debug" value="false"/>
  </include>  

  <group ns = "$(arg first_tb3)">
    <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />

    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
      <param name="publish_frequency" type="double" value="50.0" />
      <param name="tf_prefix" value="$(arg first_tb3)" />
    </node>
    
    <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-urdf -model $(arg first_tb3) -x $(arg first_tb3_x_pos) -y $(arg first_tb3_y_pos) -z $(arg first_tb3_z_pos) -Y $(arg first_tb3_yaw) -param robot_description" />
  </group>

  <group ns = "$(arg second_tb3)">
    <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />

    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
      <param name="publish_frequency" type="double" value="50.0" />
      <param name="tf_prefix" value="$(arg second_tb3)" />
    </node>

    <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-urdf -model $(arg second_tb3) -x $(arg second_tb3_x_pos) -y $(arg second_tb3_y_pos) -z $(arg second_tb3_z_pos) -Y $(arg second_tb3_yaw) -param robot_description" />
  </group>

  <group ns = "$(arg third_tb3)">
    <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />

    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
      <param name="publish_frequency" type="double" value="50.0" />
      <param name="tf_prefix" value="$(arg third_tb3)" />
    </node>

    <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-urdf -model $(arg third_tb3) -x $(arg third_tb3_x_pos) -y $(arg third_tb3_y_pos) -z $(arg third_tb3_z_pos) -Y $(arg third_tb3_yaw) -param robot_description" />
  </group>

</launch>

下面这是发布机器人各部分组件的tf变换关系

每一个group它都包含一个机器人模型载入，发布机器人各组件之间的tf变换关系的节点，还有一个机器人初始位置的设定。

然后修改m_turtlebot3_world.launch文件如下

<launch>
  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>

  <arg name="first_tb3"  default="tb3_0"/>
  <arg name="second_tb3"  default="tb3_1"/>
  <arg name="third_tb3"  default="tb3_2"/>


  <arg name="first_tb3_x_pos" default="2.0"/>
  <arg name="first_tb3_y_pos" default="-0.5"/>
  <arg name="first_tb3_z_pos" default="0.0"/>

  <arg name="second_tb3_x_pos" default="2.0"/>
  <arg name="second_tb3_y_pos" default="0.5"/>
  <arg name="second_tb3_z_pos" default="0.0"/>

  <arg name="third_tb3_x_pos" default="2.0"/>
  <arg name="third_tb3_y_pos" default="-0.8"/>
  <arg name="third_tb3_z_pos" default="0.0"/>



  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find turtlebot3_gazebo)/worlds/turtlebot3_world.world"/>
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="gui" value="true"/>
    <arg name="headless" value="false"/>
    <arg name="debug" value="false"/>
  </include>




  <group ns = "$(arg first_tb3)">
    <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />

    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
      <param name="publish_frequency" type="double" value="50.0" />
      <param name="tf_prefix" value="$(arg first_tb3)" />
    </node>

     <node pkg="gazebo_ros" type="spawn_model" name="spawn_urdf"  args="-urdf -model $(arg first_tb3) -x $(arg first_tb3_x_pos) -y $(arg first_tb3_y_pos) -z $(arg first_tb3_z_pos) -param robot_description" />
  </group>




  <group ns = "$(arg second_tb3)">
     <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />

    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
      <param name="publish_frequency" type="double" value="50.0" />
      <param name="tf_prefix" value="$(arg second_tb3)" />
    </node>

     <node pkg="gazebo_ros" type="spawn_model" name="spawn_urdf"  args="-urdf -model $(arg second_tb3) -x $(arg second_tb3_x_pos) -y $(arg second_tb3_y_pos) -z $(arg second_tb3_z_pos) -param robot_description" />
  </group>




  <group ns = "$(arg third_tb3)">
    <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />

    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
      <param name="publish_frequency" type="double" value="50.0" />
      <param name="tf_prefix" value="$(arg third_tb3)" />
    </node>


     <node pkg="gazebo_ros" type="spawn_model" name="spawn_urdf"  args="-urdf -model $(arg third_tb3) -x $(arg third_tb3_x_pos) -y $(arg third_tb3_y_pos) -z $(arg third_tb3_z_pos) -param robot_description" />
  </group>




</launch>

再次运行命令就会发现slam不会报错了

运行下面命令来开启地图合并

roslaunch turtlebot3_gazebo multi_map_merge.launch

然后开启rviz

rosrun rviz rviz -d rospack find turtlebot3_gazebo/rviz/multi_turtlebot3_slam.rviz

添加map后，选择融合的map（由于没有闭环检测，直接叠加，有点丑hhh）

但是添加机器人模型后出现如下报错

显示URDF找不到

应该是少打了些标点符号，改为

rosrun rviz rviz -d `rospack find turtlebot3_gazebo`/rviz/multi_turtlebot3_slam.rviz

如下图所示

由于已经实现了一个键盘控制多个机器人了，所以直接开启键盘遥控即可

rosrun turtlebot3_teleop turtlebot3_teleop_key

最后采用map_server来保存地图

rosrun map_server map_saver -f ~/map