ROS 多机器人建图与导航

一、环境配置

参考turtlebot3-多机交互程序:多机交互-创客智造
小车：

Ubuntu-mate16.04+ROSkinetic；
必要的bringup.launch文件（包括雷达信息、IMU、编码器信息、电器驱动），详情见bringup详解。
上位机：

Ubuntu16.04+ROSkinetic
安装多机地图合并软件包ros-kinetic-multirobot-map-merge：

sudo apt-get install ros-kinetic-multirobot-map-merge

安装turtlebot3源码：turtlebot3源码安装；
需添加turbot3源码：

git clone https://github.com/ROBOTIS-GIT/turbot3.git
cd ~/catkin_ws && catkin_make

二、 建图

2.1、建图主要命令

开启小车底盘和雷达（小车端），其他小车同理：

roslaunch clbrobot robot.launch robot_name:="robot1"

开启单个小车的slam建图并设定初始位置和建图算法（以下均在上位机端运行）：

roslaunch multi_slam multi_slam.launch robot_name:="robot1" robot_y_pos:="10.0" slam_methods:=gmapping

开启键盘控制：

roslaunch multi_slam keyboard.launch robot_name:="robot1"

Tip：以上几个机器人就得重复几次

运行多机建图并打开rviz：

roslaunch multi_slam map_merge.launch

完成多机建图后保存地图

rosrun map_server map_saver -f /home/catkin_ws/src/multi_robot/map

修改 map.yaml 中的 origin 为：

origin: [0.000000, 0.000000, 0.000000]

2.2、建图模块分析

multi_slam.launch 代码如下：

<launch>
  <arg name="robot_name" default=""/>
  <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="slam_methods" default="gmapping" doc="slam type [gmapping, hector, karto]"/>
  
  <!-- robot_model -->
  <include file="$(find robot_model)/model_robot.launch">   
    <arg name="robot_name" value="$(arg robot_name)"/>
  </include>
  
  <!-- init_pose -->
  <include file="$(find multi_slam)/launch/init_pose.launch">   
    <arg name="robot_name" value="$(arg robot_name)"/>
    <arg name="robot_x_pos"   value="$(arg robot_x_pos)"/>
    <arg name="robot_y_pos"   value="$(arg robot_y_pos)"/>
    <arg name="robot_z_pos"   value="$(arg robot_z_pos)"/>
    <arg name="robot_yaw" value="$(arg robot_yaw)"  />
  </include>
  
  <!-- slam_methods -->
  <include file="$(find multi_slam)/launch/slam.launch">   
    <arg name="robot_name" value="$(arg robot_name)"/>
    <arg name="slam_methods" value="$(arg slam_methods)"/>
  </include>

</launch>

map_merge.launch 代码如下：

<launch>
  <!-- multirobot_map_merge -->
  <node pkg="multirobot_map_merge" type="map_merge" respawn="false" name="map_merge" output="screen">
    <param name="robot_map_topic" value="map"/>
    <param name="robot_namespace" value="robot"/>
    <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.1"/>
    <param name="estimation_confidence" value="1.0"/>
  </node>

  <!-- multirobot_rviz -->
  <include file="$(find multi_slam)/launch/multi_rviz.launch"/>
</launch>

2.3、建图实现效果

rqt_tf_tree

rqt_graph

rviz效果图

建图结果

2.4、 建图github开源代码

explore自动建图的网址：http://wiki.ros.org/explore_lite

map_merge多图融合的网址：http://wiki.ros.org/multirobot_map_merge/

github代码的地址：https://github.com/hrnr/m-explore

3D地图的融合：https://github.com/hrnr/map-merge
3D多机器人导航：https://github.com/hrnr/map-merge

三、导航

3.1、 建图主要命令

开启小车底盘和雷达（小车端），其他小车同理：

roslaunch clbrobot robot.launch robot_name:="robot1"

启动地图服务程序（以下均在上位机端运行）：

roslaunch multi_navigation navigation_multi_map.launch map_file:=$HOME/tan/catkin_ws/src/multi_robot/map/map.yaml

开启小车导航navigation并设定初始位姿：

roslaunch multi_navigation navigation_multirobot.launch robot_name:="robot1"

开启rviz：

roslaunch multi_navigation navigation_rviz.launch

发布目标位置：

rostopic pub /robot1/move_base_simple/goal geometry_msgs/PoseStamped '{header: {stamp: now, frame_id: "map"}, pose: {position: {x: 0.5, y: 0.0, z: 0.0}, orientation: {w: 1.0}}}'

3.2、导航模块分析

navigation_multi_map.launch 代码如下：

<launch>
  <!-- Map server -->
  <arg name="map_file" default="/home/catkin_ws/src/multi_robot/map/map.yaml"/>
  <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)">
  </node>
</launch>        

navigation_multirobot.launch 代码如下：

<launch>
  <arg name="robot_name" default="" />
  
  <!-- robot_model -->
  <include file="$(find robot_model)/model_robot.launch">   
    <arg name="robot_name" value="$(arg robot_name)"/>
  </include>

<group ns="$(arg robot_name)">
  <!-- AMCL -->
  <arg name="custom_amcl_launch_file" default="$(find multi_navigation)/launch/amcl.multi.launch.xml"/>
  <arg name="robot_x_pos" default="0.0"/> 
  <arg name="robot_y_pos" default="0.0"/> 
  <arg name="robot_a_pos" default="0.0"/>
  <include file="$(arg custom_amcl_launch_file)">
    <arg name="global_frame_id" value="/map"/>
    <arg name="odom_frame_id"   value="$(arg robot_name)/odom"/>
    <arg name="base_frame_id"   value="$(arg robot_name)/base_footprint"/> 
    <arg name="initial_pose_x" value="$(arg robot_x_pos)"/>
    <arg name="initial_pose_y" value="$(arg robot_y_pos)"/>
    <arg name="initial_pose_a" value="$(arg robot_a_pos)"/>
  </include>
  
  <!-- move_base -->
  <arg name="custom_param_file" default="$(find multi_navigation)/param/dummy.yaml"/>
  <include file="$(find multi_navigation)/launch/move_base_robot.launch.xml">
    <arg name="global_frame_id" value="/map"/>
    <arg name="odom_frame_id"   value="/$(arg robot_name)/odom"/>
    <arg name="base_frame_id"   value="/$(arg robot_name)/base_footprint"/>
    <arg name="odom_topic" value="/$(arg robot_name)/odom" />
    <arg name="laser_topic" value="/$(arg robot_name)/scan" />
    <arg name="cmd_topic" value="/$(arg robot_name)/cmd_vel" />
    <arg name="custom_param_file" value="$(arg custom_param_file)"/>
  </include>
</group>
</launch>

amcl.multi.launch.xml 代码如下：

<launch>
  <arg name="robot_name"      default="" />
  <arg name="use_map_topic"   default="true"/>
  <arg name="scan_topic"      default="scan"/> 
  <arg name="initial_pose_x"  default="0.0"/>
  <arg name="initial_pose_y"  default="0.0"/>
  <arg name="initial_pose_a"  default="0.0"/>
  <arg name="odom_frame_id"   default="odom"/>
  <arg name="base_frame_id"   default="base_footprint"/>
  <arg name="global_frame_id" default="/map"/>
  <node pkg="amcl" type="amcl" name="amcl">
    <param name="use_map_topic"             value="$(arg use_map_topic)"/>
    <!-- Publish scans from best pose at a max of 10 Hz -->
    <param name="odom_model_type"           value="diff"/>
    <param name="odom_alpha5"               value="0.1"/>
    <param name="gui_publish_rate"          value="10.0"/>
    <param name="laser_max_beams"             value="60"/>
    <param name="laser_max_range"           value="12.0"/>
    <param name="min_particles"             value="500"/>
    <param name="max_particles"             value="2000"/>
    <param name="kld_err"                   value="0.05"/>
    <param name="kld_z"                     value="0.99"/>
    <param name="odom_alpha1"               value="0.25"/>
    <param name="odom_alpha2"               value="0.25"/>
    <!-- translation std dev, m -->
    <param name="odom_alpha3"               value="0.25"/>
    <param name="odom_alpha4"               value="0.25"/>
    <param name="laser_z_hit"               value="0.5"/>
    <param name="laser_z_short"             value="0.05"/>
    <param name="laser_z_max"               value="0.05"/>
    <param name="laser_z_rand"              value="0.5"/>
    <param name="laser_sigma_hit"           value="0.2"/>
    <param name="laser_lambda_short"        value="0.1"/>
    <param name="laser_model_type"          value="likelihood_field"/>
    <!-- <param name="laser_model_type" value="beam"/> -->
    <param name="laser_likelihood_max_dist" value="2.0"/>
    <param name="update_min_d"              value="0.2"/>
    <param name="update_min_a"              value="0.2"/>
    <param name="odom_frame_id"             value="$(arg odom_frame_id)"/> 
    <param name="base_frame_id"             value="$(arg base_frame_id)"/> 
    <param name="global_frame_id"           value="$(arg global_frame_id)"/>
    <param name="resample_interval"         value="1"/>
    <!-- Increase tolerance because the computer can get quite busy -->
    <param name="transform_tolerance"       value="1.25"/>
    <param name="recovery_alpha_slow"       value="0.001"/>
    <param name="recovery_alpha_fast"       value="0.1"/>
    <param name="initial_pose_x"            value="$(arg initial_pose_x)"/>
    <param name="initial_pose_y"            value="$(arg initial_pose_y)"/>
    <param name="initial_pose_a"            value="$(arg initial_pose_a)"/>
    <remap from="scan"                      to="$(arg scan_topic)"/>
    <remap from="static_map" to="/static_map" />
    <remap from="map" to="/map" />
  </node>
</launch>

move_base_robot.launch.xml 代码如下：

<launch>
  <arg name="odom_frame_id"   default="odom"/>
  <arg name="base_frame_id"   default="base_footprint"/>
  <arg name="global_frame_id" default="/map"/>
  <arg name="odom_topic" default="odom" />
  <arg name="laser_topic" default="scan" />
  <arg name="cmd_topic" default="cmd_vel" />
  <arg name="custom_param_file" default="$(find multi_navigation)/param/dummy.yaml"/>
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <rosparam file="$(find multi_navigation)/param/tank/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find multi_navigation)/param/tank/costmap_common_params.yaml" command="load" ns="local_costmap" />   
    <rosparam file="$(find multi_navigation)/param/tank/local_costmap_params.yaml" command="load" />   
    <rosparam file="$(find multi_navigation)/param/tank/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find multi_navigation)/param/tank/move_base_params.yaml" command="load" />
    <rosparam file="$(find multi_navigation)/param/tank/base_local_planner_params.yaml" command="load" />
    <!-- external params file that could be loaded into the move_base namespace -->
    <rosparam file="$(arg custom_param_file)" command="load" />
    <!-- reset frame_id parameters using user input data -->
    <param name="global_costmap/global_frame" value="$(arg global_frame_id)"/>
    <param name="global_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
    <param name="local_costmap/global_frame" value="$(arg global_frame_id)"/>
    <param name="local_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
    <param name="global_costmap/obstacle_layer/scan/topic" value="$(arg laser_topic)"/>
    <param name="local_costmap/obstacle_layer/scan/topic" value="$(arg laser_topic)"/>
    <remap from="cmd_vel" to="$(arg cmd_topic)"/>
    <remap from="odom" to="$(arg odom_topic)"/>
    <remap from="scan" to="$(arg laser_topic)"/>
  </node>
</launch>

3.3、导航实现效果

rqt_tf_tree

rqt_graph

多机导航