ROS仿真之多个turtlebot2导航

ROS仿真之多个turtlebot2导航

多个机器人导航仿真主要分为两个部分，一个是gazbo中加载多机器人，另一个是在rviz中显示多机器人并可以控制。
1.gazebo加载多机器人
打开终端输入：

source devel/setup.bash 

export TURTLEBOT3_MODEL=waffle

roslaunch turtlebot3_gazebo m_turtlebot3_world.launch 

最主要的就是修改启动launch文件，具体内容如下：
原turtlebot3的launch文件：

<launch>
  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
  <arg name="x_pos" default="-2.0"/>
  <arg name="y_pos" default="-0.5"/>
  <arg name="z_pos" 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>

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

  <node pkg="gazebo_ros" type="spawn_model" name="spawn_urdf"  args="-urdf -model turtlebot3_$(arg model) -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -param robot_description" />
</launch>

修改后的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/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="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="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="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>

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

source devel/setup.bash 

export TURTLEBOT3_MODEL=waffle

roslaunch turtlebot3_navigation multi_navigation.launch

同样，最主要的是修改launch文件。
原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/cloister_gmapping.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文件内容如下：

<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/m_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="2.0"/>
  <arg name="first_tb3_y_pos" default="-0.5"/>
  <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/m_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="2.0"/>
  <arg name="second_tb3_y_pos" default="0.5"/>
  <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/m_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.8"/>
  <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/multi_turtlebot3_navigation.rviz"/>
  </group>
</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>

其内容主要包括地图加载mapserver、ACML、movebase、rviz显示插件。对比发现，也是对每个turtlebot加了ns属性，原因是gazebo仿真中已经对每个小车设置了属性名称，在这里也要一一对应上，使得tf树能正确显示，tf树的正确变换如下：

仿真效果如下：