ROS Turtlebot3多机器人编队导航仿真

---

前言

前面已经实现了在gazebo仿真环境中机器人一字型编队、三角形编队、N字型编队等仿真，接下来考虑多机器人编队在编队行进过程中的避障问题，通过在RVIZ中加载多个机器人使他们能分别进行全局和局部路径规划，来进行避障。

---

一、Gzazebo中加载多台Turtlebot3机器人

在前面的文章中也提到过在gazebo仿真环境中加载多个机器人主要是修改启动gazebo仿真环境的launch文件。
原Turtlebot3 launch文件

<launch><env name="GAZEBO_RESOURCE_PATH" value="$(find turtlebot3_gazebo)/models/turtlebot3_autorace/ground_picture" /><arg name="x_pos" default="0.245"/><arg name="y_pos" default="-1.787"/><arg name="z_pos" default="0"/>  <include file="$(find gazebo_ros)/launch/empty_world.launch"><arg name="world_name" value="$(find turtlebot3_gazebo)/worlds/turtlebot3_autorace.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>  <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_burger_for_autorace.urdf.xacro" /><node pkg="gazebo_ros" type="spawn_model" name="spawn_urdf" args="-urdf -model turtlebot3_burger -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -param robot_description" /></launch>

添加多台机器人的launch文件

<?xml version="1.0"?>
<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=" 0.0"/><arg name="first_tb3_y_pos" default=" 0.0"/><arg name="first_tb3_z_pos" default=" 0.0"/><arg name="first_tb3_yaw"   default=" 0.0"/><arg name="second_tb3_x_pos" default=" -1.0"/><arg name="second_tb3_y_pos" default=" 0.0"/><arg name="second_tb3_z_pos" default=" 0.0"/><arg name="second_tb3_yaw"   default=" 0.0"/><arg name="third_tb3_x_pos" default=" -2.0"/><arg name="third_tb3_y_pos" default=" 0.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/cloister.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>

对比发现，修改后的launch文件增加了三个tb3的ns属性，并为其设置了初始位姿，设置了ns属性后才使得每个仿真小车发布带有对应属性的节点名称，例如/tb3_0/odom、/tb3_0/base_link、/tb3_1/odom、/tb3_1/base_link。这样能防止TF树错乱而导致的运行出错。

二、RVIZ中加载多个Turtlebot3机器人

与Gazebo中加载多个Turtlebot3机器人相比，RVIZ需要修改的内容多了些。首先修改turtlebot3_navigation.launch
原文件：

<launch><!-- Arguments --><arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/><arg name="map_file" default="$(find turtlebot3_navigation)/maps/map.yaml"/><arg name="open_rviz" default="true"/><arg name="move_forward_only" default="false"/><!-- Turtlebot3 --><include file="$(find turtlebot3_bringup)/launch/turtlebot3_remote.launch"><arg name="model" value="$(arg model)" /></include><!-- Map server --><node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)"/><!-- AMCL --><include file="$(find turtlebot3_navigation)/launch/amcl.launch"/><!-- move_base --><include file="$(find turtlebot3_navigation)/launch/move_base.launch"><arg name="model" value="$(arg model)" /><arg name="move_forward_only" value="$(arg move_forward_only)"/></include><!-- rviz --><group if="$(arg open_rviz)"> <node pkg="rviz" type="rviz" name="rviz" required="true"args="-d $(find turtlebot3_navigation)/rviz/turtlebot3_navigation.rviz"/></group>
</launch>

修改后：

<launch><!-- Arguments --><arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/><arg name="open_rviz" default="true"/><arg name="move_forward_only" default="false"/><arg name="first_tb3"  default="tb3_0"/><arg name="second_tb3" default="tb3_1"/><arg name="third_tb3"  default="tb3_2"/><!-- Map server --><arg name="map_file" default="$(find turtlebot3_navigation)/maps/cloister_gmapping.yaml"/><node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)"/><group ns = "$(arg first_tb3)"><!-- Map server --><node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)"/><!-- Turtlebot3 --><include file="$(find turtlebot3_bringup)/launch/turtlebot3_remote.launch"><arg name="model" value="$(arg model)" /><arg name="multi_robot_name" value="$(arg first_tb3)" /></include><arg name="first_tb3_x_pos" default="0.0"/><arg name="first_tb3_y_pos" default="0.0"/><arg name="first_tb3_z_pos" default="0.0"/><!-- AMCL --><include file="$(find turtlebot3_navigation)/launch/multi_amcl.launch"><arg name="global_frame_id" value="map"/><arg name="odom_frame_id"   value="$(arg first_tb3)/odom"/><arg name="base_frame_id"   value="$(arg first_tb3)/base_footprint"/> <arg name="initial_pose_x" value="$(arg first_tb3_x_pos)"/><arg name="initial_pose_y" value="$(arg first_tb3_y_pos)"/><arg name="initial_pose_a" value="$(arg first_tb3_z_pos)"/></include><!-- move_base --><include file="$(find turtlebot3_navigation)/launch/multi_move_base.launch"><arg name="global_frame_id" value="map"/><arg name="odom_frame_id"   value="$(arg first_tb3)/odom"/><arg name="base_frame_id"   value="$(arg first_tb3)/base_footprint"/><arg name="odom_topic" value="/$(arg first_tb3)/odom" /><arg name="laser_topic" value="/$(arg first_tb3)/scan" /><arg name="cmd_vel_topic" value="/$(arg first_tb3)/cmd_vel" /><arg name="model" value="$(arg model)" /><arg name="move_forward_only" value="$(arg move_forward_only)"/></include>
</group><group ns = "$(arg second_tb3)"><!-- Map server --><node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)"/><!-- Turtlebot3 --><include file="$(find turtlebot3_bringup)/launch/turtlebot3_remote.launch"><arg name="model" value="$(arg model)" /><arg name="multi_robot_name" value="$(arg second_tb3)" /></include><arg name="second_tb3_x_pos" default="-1.0"/><arg name="second_tb3_y_pos" default="0.0"/><arg name="second_tb3_z_pos" default="0.0"/><!-- AMCL --><include file="$(find turtlebot3_navigation)/launch/multi_amcl.launch"><arg name="global_frame_id" value="map"/><arg name="odom_frame_id"   value="$(arg second_tb3)/odom"/><arg name="base_frame_id"   value="$(arg second_tb3)/base_footprint"/> <arg name="initial_pose_x" value="$(arg second_tb3_x_pos)"/><arg name="initial_pose_y" value="$(arg second_tb3_y_pos)"/><arg name="initial_pose_a" value="$(arg second_tb3_z_pos)"/></include><!-- move_base --><include file="$(find turtlebot3_navigation)/launch/multi_move_base.launch"><arg name="global_frame_id" value="map"/><arg name="odom_frame_id"   value="$(arg second_tb3)/odom"/><arg name="base_frame_id"   value="$(arg second_tb3)/base_footprint"/><arg name="odom_topic" value="/$(arg second_tb3)/odom" /><arg name="laser_topic" value="/$(arg second_tb3)/scan" /><arg name="cmd_vel_topic" value="/$(arg second_tb3)/cmd_vel" /><arg name="model" value="$(arg model)" /><arg name="move_forward_only" value="$(arg move_forward_only)"/></include>
</group><group ns = "$(arg third_tb3)"><!-- Map server --><node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)"/><!-- Turtlebot3 --><include file="$(find turtlebot3_bringup)/launch/turtlebot3_remote.launch"><arg name="model" value="$(arg model)" /><arg name="multi_robot_name" value="$(arg third_tb3)" /></include><arg name="third_tb3_x_pos" default="-2.0"/><arg name="third_tb3_y_pos" default="0.0"/><arg name="third_tb3_z_pos" default="0.0"/><!-- AMCL --><include file="$(find turtlebot3_navigation)/launch/multi_amcl.launch"><arg name="global_frame_id" value="map"/><arg name="odom_frame_id"   value="$(arg third_tb3)/odom"/><arg name="base_frame_id"   value="$(arg third_tb3)/base_footprint"/> <arg name="initial_pose_x" value="$(arg third_tb3_x_pos)"/><arg name="initial_pose_y" value="$(arg third_tb3_y_pos)"/><arg name="initial_pose_a" value="$(arg third_tb3_z_pos)"/></include><!-- move_base --><include file="$(find turtlebot3_navigation)/launch/multi_move_base.launch"><arg name="global_frame_id" value="map"/><arg name="odom_frame_id"   value="$(arg third_tb3)/odom"/><arg name="base_frame_id"   value="$(arg third_tb3)/base_footprint"/><arg name="odom_topic" value="/$(arg third_tb3)/odom" /><arg name="laser_topic" value="/$(arg third_tb3)/scan" /><arg name="cmd_vel_topic" value="/$(arg third_tb3)/cmd_vel" /><arg name="model" value="$(arg model)" /><arg name="move_forward_only" value="$(arg move_forward_only)"/></include>
</group><!-- rviz --><group if="$(arg open_rviz)"> <node pkg="rviz" type="rviz" name="rviz" required="true"args="-d $(find turtlebot3_navigation)/rviz/turtlebot3_navWjx.rviz"/></group>
</launch>

turtlebot3_navigation.launch中引用的move_base.launch和amcl.launch文件也需要修改
move_base.launch
原文件：

<launch><!-- Arguments --><arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/><arg name="cmd_vel_topic" default="/cmd_vel" /><arg name="odom_topic" default="odom" /><arg name="move_forward_only" default="false"/><!-- move_base --><node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen"><param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" /><rosparam file="$(find turtlebot3_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="global_costmap" /><rosparam file="$(find turtlebot3_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="local_costmap" /><rosparam file="$(find turtlebot3_navigation)/param/local_costmap_params.yaml" command="load" /><rosparam file="$(find turtlebot3_navigation)/param/global_costmap_params.yaml" command="load" /><rosparam file="$(find turtlebot3_navigation)/param/move_base_params.yaml" command="load" /><rosparam file="$(find turtlebot3_navigation)/param/dwa_local_planner_params_$(arg model).yaml" command="load" /><remap from="cmd_vel" to="$(arg cmd_vel_topic)"/><remap from="odom" to="$(arg odom_topic)"/><param name="DWAPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" /></node>
</launch>

修改后（multi_move_base.launch）：

<launch><!-- Arguments --><arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/><arg name="odom_frame_id"   default="odom"/><arg name="base_frame_id"   default="base_footprint"/><arg name="global_frame_id" default="map"/><arg name="cmd_vel_topic" default="/cmd_vel" /><arg name="odom_topic" default="odom" /><arg name="laser_topic" default="scan" /><arg name="move_forward_only" default="false"/><!-- move_base --><node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen"><param name="base_global_planner" value="global_planner/GlobalPlanner"/><param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS"/><rosparam file="$(find turtlebot3_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="global_costmap" /><rosparam file="$(find turtlebot3_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="local_costmap" /><rosparam file="$(find turtlebot3_navigation)/param/local_costmap_params.yaml" command="load" /><rosparam file="$(find turtlebot3_navigation)/param/global_costmap_params.yaml" command="load" /><rosparam file="$(find turtlebot3_navigation)/param/move_base_params.yaml" command="load" /><rosparam file="$(find turtlebot3_navigation)/param/dwa_local_planner_params_$(arg model).yaml" 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 odom_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_vel_topic)"/><remap from="odom" to="$(arg odom_topic)"/><remap from="scan" to="$(arg laser_topic)"/><param name="DWAPlanner/min_vel_x" value="0.0" if="$(arg move_forward_only)" /></node>
</launch>

amcl.launch
原文件：

<launch><!-- Arguments --><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"/><!-- AMCL --><node pkg="amcl" type="amcl" name="amcl"><param name="min_particles"             value="500"/><param name="max_particles"             value="3000"/><param name="kld_err"                   value="0.02"/><param name="update_min_d"              value="0.20"/><param name="update_min_a"              value="0.20"/><param name="resample_interval"         value="1"/><param name="transform_tolerance"       value="0.5"/><param name="recovery_alpha_slow"       value="0.00"/><param name="recovery_alpha_fast"       value="0.00"/><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)"/><param name="gui_publish_rate"          value="50.0"/><remap from="scan"                      to="$(arg scan_topic)"/><param name="laser_max_range"           value="3.5"/><param name="laser_max_beams"           value="180"/><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_likelihood_max_dist" value="2.0"/><param name="laser_model_type"          value="likelihood_field"/><param name="odom_model_type"           value="diff"/><param name="odom_alpha1"               value="0.1"/><param name="odom_alpha2"               value="0.1"/><param name="odom_alpha3"               value="0.1"/><param name="odom_alpha4"               value="0.1"/><param name="odom_frame_id"             value="odom"/><param name="base_frame_id"             value="base_footprint"/></node>
</launch>

修改后（multi_amcl.launch读入数据）：

<launch><!-- Arguments --><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"/><!-- AMCL --><node pkg="amcl" type="amcl" name="amcl"><param name="use_map_topic"             value="$(arg use_map_topic)"/><param name="min_particles"             value="500"/><param name="max_particles"             value="3000"/><param name="kld_err"                   value="0.02"/><param name="update_min_d"              value="0.20"/><param name="update_min_a"              value="0.20"/><param name="resample_interval"         value="1"/><param name="transform_tolerance"       value="0.5"/><param name="recovery_alpha_slow"       value="0.00"/><param name="recovery_alpha_fast"       value="0.00"/><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)"/><param name="gui_publish_rate"          value="50.0"/><remap from="scan"                      to="$(arg scan_topic)"/><param name="laser_max_range"           value="3.5"/><param name="laser_max_beams"           value="180"/><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_likelihood_max_dist" value="2.0"/><param name="laser_model_type"          value="likelihood_field"/><param name="odom_model_type"           value="diff"/><param name="odom_alpha1"               value="0.1"/><param name="odom_alpha2"               value="0.1"/><param name="odom_alpha3"               value="0.1"/><param name="odom_alpha4"               value="0.1"/><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)"/></node>
</launch>

三.多机器人编队导航

多机器人编队的仿真在前面文章中叙述过：多机器人三角形编队的实现，编队的程序不需要修改，可以直接运行。
启动gazebo仿真环境(launch 自己的文件名)：

roslaunch turtlebot3_gazebo multi3_turtlebot3.launch

启动导航节点（RVIZ）

roslaunch  turtlebot3_navigation turtlebot3_navigation

启动编队程序：

roslaunch turtlebot3_teams_wang  turtlebot3_teams_follow_wang.launch

---

总结

简单总结了Turtlebot3多机器人编队仿真，在实现的过程中需要对RVIZ仿真环境进行配置，这个花费了我很长时间，后面我会专门的写一下，主要是添加多个2D nav Goal/2D pose estimate，以及导航、laser、全局路径/局部路径话题的订阅。