Slam learning notes - build a complete gazebo multi machine simulation slam from scratch (II)

introduction

Then I will talk about the content of the previous lesson , We use the single machine in the last lesson slam Map of construction . This time we introduce several robots , And use multirobot_map_merge This package is used for real-time fusion map . The source file is in the gitee In the link .

To write map_merge file

This file is used to start map_merge service , Accept all with robot_namespace At the beginning map file , And fuse through the initial position coordinates , If you do not specify the initial position coordinates , Fusion may depend on features for its , It's not going to work .

<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"/>
    <!--  Accept all with tb3 At the beginning 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>

To write two_robots_slam.launch file

Go successively gazebo Put two robots in the environment , And start the map fusion node

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

    <group if="$(arg simulation)" >
        <!--  Open the simulation world  -->
        <include file="$(find gazebo_tutorials)/launch/create_world.launch" />
         <!--  Place the robot  -->
        <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>
         <!--  Place the robot  -->
        <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>
    <!--  Turn on 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>

    <!--  Start the map fusion node and join 1 Robot map information  -->
     <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>
    <!--  Join in 2 Robot map information  -->
    <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 To configure

We click add, choice map, then topic Set to /map, It's the integrated map
Then join in laserscan Scan point cloud and axes Robot coordinates

What we need to do this time rviz There are more configurations , So we save it as a configuration file , Each start launch Files run automatically .
The way to save is to click on the upper left corner file,save config as, Then write rviz file

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

Then we just need to run directly roslaunch gazebo_tutorials two_robots_slam.launch, You can start it
Finally generated rqt_graph, Clear structure

Use teleop Control and map_saver Save the map

Basically the same as before , Remember to change ROS_NAMESPACE, Use the following command to tb3_0 To operate
ROS_NAMESPACE=tb3_0 rosrun teleop_twist_keyboard teleop_twist_keyboard.py
Use the following command to save the map
rosrun map_server map_saver -f xxx/Map