前面的文章分别介绍了如何在Gazebo中实现多机器人仿真以及多移动机器人编队的研究。在ROS中，rviz也是开发者们常用到的一款数据可视化工具。ROS中有很多的数据，包括地图数据、图像数据等，数据形态往往不利于开发者感受其描述的内容，所以常常需要将数据可视化显示，Rviz就是针对可视化需求的一款显示多种数据的一款三维可视化平台——Rviz。Rviz可以很好的兼容各种基于ROS框架的机器人平台，机器人及各种周围物件环境的属性（尺寸、质量、位置、材质、关节等）可以用XML文件来描述，并且图形化地呈现在界面上。同时还可以通过图形化的方式实时显示机器人传感器信息，位置状态，周围环境变化等。 本文将在ROS系统的基础上，在gazebo中实现多机器人的仿真，并在rviz中实现其中任一机器人的自主导航功能。

1.ROS中自主导航框架介绍

ROS中有专门服务于机器人自主导航的功能包，进行导航需要用到的三个包是： 1）move_base包：根据地图和目标点信息进行路径规划，使移动机器人到达目标点； 2）gampping包：根据激光雷达数据（或深度摄像头模拟数据）构建地图； 3）amcl包：完成机器人在已有地图中的定位。 其中最重要的是move_base包，主要包括两个部分：全局路径规划（global planner）和局部路径规划（local planner）：根据周围障碍物进行避障路线规划。 图1-1为其结构框图。 结构框图 白色框内是ROS为用户提供的组件，必须先准备好；灰色框内的是可选使用组件；蓝色框内的用户所用机器人平台应该提供的组件。下面对灰色框和蓝色框内的组件作简单说明：
  • sensor transforms：此组件功能是实现移动机器人传感器和控制中心之间的tf坐标变换。
  • sensor sources：此组件为机器人导航传感器的数据输入接口
  • odometry source：此组件为里程计数据输入接口。里程计反应机器人控制中心相对于地图的运动距离，从而完成仿真和定位。
  • base controller：此组件将导航决策后的数据封装为具体的运动控制数据Twist（包括线速度和角速度），发布给机器人平台。
  • map_server：此组件用于发布地图。
gampping包在实际应用中可以通过机器人获取的激光数据或深度数据生成地图，在仿真中也可以在Gazebo环境中获取数据并生成地图，然后在Rviz中可视化显示，并进行导航等一系列后续操作。Amcl（adaptive Monte Carlo Localization）包是二维环境下的概率定位系统，采用的是自适应的蒙特卡罗定位法 在实际应用中，ROS主要负责地图构建和路径规划的功能。图1-2为ROS层关于自主导航功能的软件结构。 自主导航功能的软件结构 移动机器人的传感器数据（如编码器数据，IMU数据，摄像头数据等）在经过融合滤波等一系列处理后，会转化为机器人的定位数据，这些定位数据将不断地传入ROS层。ROS层接到数据后，会用相应的底盘定位发布节点发布机器人定位，并经过TF关系的转换。在SLAM建图时，SLAM节点会结合获取到的雷达激光数据以及机器人定位数据，确定机器人所处位置，提取周围环境特征并建图。在执行路径规划时，ROS层会根据周围环境的信息以及机器人的定位做出判断，从而生成路径规划的结果，并将导航路径做平滑曲线处理发布至运动控制节点，由运动控制节点将路径解析为移动机器人的运动控制命令并发布，从而控制机器人的运动。 在Gazebo仿真环境中，机器人的定位可以直接通过Gazebo中的插件获得。 下图1-3是仿真中移动机器人实现自主导航时，用ROS中rqt工具rqt_graph显示出来的节点关系图。 节点joint_state_publisher将非固定关节信息/joint_states发布至节点robot_state_publisher，该节点作用是处理机器人坐标系和其他物体坐标系之间的关系。robot_state_publisher节点将tf变换发布至路径规划节点move_base与以及定位节点amcl。move_base同时接收来自地图服务器map_server的地图信息/map和来自仿真环境/Gazebo发布的里程计信息/odom，从而对机器人的位置信息以及周围环境信息做出判断，结合给定的目标点/move_base/goal，生成全局规划路线，并生成速度控制指令/cmd_vel发送至仿真环境中来控制小车的运动。定位节点amcl接收机器人坐标系信息以及激光扫描数据/scan，并将其转换为里程计结果（odom_frame_id），通过tf关系发布到move_base节点中，以实现地图中机器人定位的调整。

2.效果展示

运行指令： 启动仿真环境：1roslaunch ares_gazebo ares_cloister_gazebo.launch 打开rviz,进行导航：roslaunch ares_navigation navigation_demo.launch 运行时的rqt_graph:

3.具体实现步骤

3.1 tf关系设置

多机器人的tf关系发布不带命名空间，故需要在gazebo启动launch文件中加入tf_prefix 
<!-- 运行robot_state_publisher节点，发布tf  -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"  output="screen" >
        <param name="publish_frequency" type="double" value="50.0" />
        <param name="tf_prefix" value="ares1" />
</node>
这样则机器人内部的tf转换都会加有ares1（ares2）的前缀以区分

3.2 运动插件加入命名前缀

因为gazebo的插件libgazebo_ros_planar_move.so会发布这个机器人的基础坐标系相对于odom的tf关系，而这个发布的tf是不会带ns属性的。（这个问题是困扰了我最久的问题，总会莫名其妙发一个无前缀的base_link，并且值在ares1/base_link和ares2/base_link之间跳动，导致rviz里面的模型抖动和tf树断裂） 解决办法：在gazebo启动launch文件中加入参数，传入机器人模型加载xcaro文件中。（向xcaro文件中传参方法在网上都没说) 参考realsense工程 https://github.com/IntelRealSense/realsense-ros） 1）在gazebo启动launch文件中： 
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find ares_description)/urdf/ares_laser.xacro' bodyframe:=ares1 ns:=ares1" />
2）在ares_base.xacro文件中： 
<plugin name="mecanum_controller" filename="libgazebo_ros_planar_move.so">                               
<commandTopic>cmd_vel</commandTopic>
<odometryTopic>odom</odometryTopic>
           <odometryFrame>$(arg bodyframe)/odom</odometryFrame>
<leftFrontJoint>wheel_lf_joint</leftFrontJoint 
<rightFrontJoint>wheel_rf_joint</rightFrontJoint>
<leftRearJoint>wheel_lb_joint</leftRearJoint>
<rightRearJoint>wheel_rb_joint</rightRearJoint>
<odometryRate>20.0</odometryRate>
<robotBaseFrame>$(arg bodyframe)/base_link</robotBaseFrame 
<!--robotBaseFrame>base_link</robotBaseFrame-->
</plugin>
这样就实现了参数传递，插件 libgazebo_ros_planar_move.so 发布的>base_link带有前缀。

3.3 多机器人命名空间设置

在move_base启动文件中加入地图的frame_id以及各种重映射，以确保各种包的信息能够和我们命名空间内的信息互通 navigation_demo.launch文件: 
    <launch>
    <!-- 设置地图的配置文件 -->
    <arg name="map" default="cloister_gmapping.yaml" />
        <!-- 运行地图服务器，并且加载设置的地图-->
        <node name="map_server" pkg="map_server" type="map_server" args="$(find ares_navigation)/maps/$(arg map)"/>
        <param name="frame_id" value="ares1/map" />
        <remap from="/cmd_vel" to="ares1/cmd_vel"/>
        <remap from="/scan" to="ares1/scan"/>
        <remap from="ares1/odom" to="odom"/>
    <!--node pkg="tf" type="static_transform_publisher" name="map_odom_broadcaster1" args="0 0 0 0 0 0 0 map odom 100" /-->
    <!--node pkg="tf" type="static_transform_publisher" name="odom_base_broadcaster1" args="0 0 0 0 0 0 0 odom ares1/base_link 100" /-->
        <!-- 运行move_base节点 -->
        <include file="$(find ares_navigation)/launch/move_base.launch"/>
        <!-- 启动AMCL节点 -->
        <include file="$(find ares_navigation)/launch/amcl.launch" />
        <!-- 运行rviz -->
        <node pkg="rviz" type="rviz" name="rviz" args="-d $(find ares_navigation)/rviz/nav.rviz"/>
    </launch>

3.4 导航及定位功能包配置

改写amcl.launch，move_base.launch文件(那几个加载的yaml文件)中关于base_link的参数，使其在rviz环境中能够对上我们所定义的ares1/base_link。（在我的工程中需要用到ares1的自主导航） amcl.launch中： 
<arg name="scan_topic" default=" /scan"/>改写为<arg name="scan_topic" default="ares1/scan"/>
<param name="base_frame_id" value=" /base_link"/>改写为<param name="base_frame_id" value="ares1/base_link"/>
move_base.launch中所用到的所有yaml文件中，base_link全部改为ares1/base_link，否则tf_tree无法正常连接。

4.总结与展望

前两章文章介绍了如何在gazebo中实现多机器人仿真，以及如何在gazebo中实现多机器人编队。这一篇文章主要介绍在gazebo中实现多机器人仿真的基础上，使其中任一机器人实现自主导航功能。这个功能听起来简单，但是其实是这些工作中耗费时间最长的，原因有二： 1）网上的教程资料很少。关于在ROS环境下，多机器人仿真中实现导航的相关介绍几乎没有，大部分工作也是参考了很多github的项目才能最终完成； 2）自主导航中的消息很多，且tf_tree要求严格。在实现自主导航功能时，不仅有运动控制指令cmd_vel，机器人定位等基本的消息，还有点云数据、位姿估计，导航目标等消息，这些消息都为默认消息，不带有命名空间，与加了ns属性多机器人系统很难配合。（作者在多机器人的工作学习中也逐渐发现，ROS1对多机器人系统其实并不友好，很多ROS中的经典功能在多机器人系统上实现起来都有些吃力，期待ROS2会在这个问题上有所改进） 在学习和写文过程中，已经有许多同学加我qq问问题，基本上最后也都得以解决。作者本身也是学生，学习工作及文章多有不足疏漏之处，希望大家来一起学习探讨！