在gazebo中实现的三角形编队

编队控制框架：基于ROS的多机器人编队仿真 - 古月居

三角形编队方案：tf/Tutorials/Adding a frame (C++) - ROS Wiki

        如wiki上的方法，在原功能包中增加了frame_tf_broadcaster.cpp文件并添加编译，使两个跟随者小车跟随虚拟坐标。两个虚拟坐标分别在领航者小车的左边和右边。

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf_broadcaster");
  ros::NodeHandle node;

  tf::TransformBroadcaster br1, br2;
  tf::Transform transform1, transform2;

  ros::Rate rate(10.0);
  while (node.ok()){
    transform1.setOrigin( tf::Vector3(0.0, 0.5, 0.0) );
    transform1.setRotation( tf::Quaternion(0, 0, 0, 1) );
    transform2.setOrigin( tf::Vector3(0.0, -0.5, 0.0) );
    transform2.setRotation( tf::Quaternion(0, 0, 0, 1) );
    br1.sendTransform(tf::StampedTransform(transform1, ros::Time::now(), "ares1", "carrot1"));
    br2.sendTransform(tf::StampedTransform(transform2, ros::Time::now(), "ares1", "carrot2"));
    rate.sleep();
  }
  return 0;
};

         在slave_tf_listener2.cpp和slave_tf_listener3.cpp两个文件中更改监听的对象，由ares1分别变为carrot1和carrot2。

        在实现一字型编队的原程序中，ares3的线速度慢于ares2，这里修改为一致。

        距离较近时领航者和跟随者小车速度相同，形成稳定的三角形编队。