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