ROS学习日志：在gazebo中机器人进行三角形编队

在gazebo中机器人进行三角形编队

前言

前文已经阐述了如何完成一字型的编队，内容实质上是以tf通信方式发布坐标，跟随者进行收听，再对坐标数据进行处理，完成一字型编队，而三角形编队与一字形编队唯一不同的地方是采用了虚拟结构的方式，以tf通信方式发布虚拟坐标，把编队问题转换成追踪目标点的问题。

代码实现

（1）回调函数

只需要在第一个小车中完成对回调函数的修改，完成对虚拟坐标的发布，虚拟坐标是相对于领航机器人的位置，可以随意调整。

void multiThreadListener_1::poseCallback0(const nav_msgs::Odometry::ConstPtr& msg)
{
	// 创建tf的广播器
	static tf::TransformBroadcaster br;
	static tf::TransformBroadcaster br0;
	static tf::TransformBroadcaster br1;

	// 初始化tf数据
	tf::Transform transform;
	tf::Transform transform0;
	tf::Transform transform1;

	transform.setOrigin( tf::Vector3(msg->pose.pose.position.x, msg->pose.pose.position.y, 0.0) );
	double roll, pitch, yaw;
	tf::Quaternion q;
	tf::Quaternion quat;
	tf::quaternionMsgToTF(msg->pose.pose.orientation, quat);
  	tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
	q.setRPY(0.0, 0.0, yaw);
	transform.setRotation(q);
	// 广播world与海龟坐标系之间的tf数据
	br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "tb3_0"));

	transform0.setOrigin( tf::Vector3(x+0.5, y-1.0, 0.0) );//初始化  相距0.6m,xunizuobiao x,yzhi
	transform0.setRotation( tf::Quaternion(0, 0, 0, 1) );
	br0.sendTransform(tf::StampedTransform(transform0, ros::Time::now(), "world", "virtual_1"));
	transform1.setOrigin( tf::Vector3(x-0.5, y-1.0, 0.0) );//初始化  相距0.6m,xunizuobiao x,yzhi
	transform1.setRotation( tf::Quaternion(0, 0, 0, 1) );
	br1.sendTransform(tf::StampedTransform(transform1, ros::Time::now(), "world", "virtual_1"));
}

（2）tf接收函数

listerner程序与之前的大致相同，因为只有追随目标变，只需要修改跟随者的跟随目标即可，修改区域代码如下：

listener.waitForTransform("/tb3_1", "/virtual_0", ros::Time(0), ros::Duration(3.0));
listener.lookupTransform("/tb3_1", "/virtual_0", ros::Time(0), transformfl);

listener.waitForTransform("/tb3_2", "/virtual_1", ros::Time(0), ros::Duration(3.0));
listener.lookupTransform("/tb3_2", "/virtual_1", ros::Time(0), transformfl);

总结

理解了古月居入门视频的tf发布与接听后，实现小车的编队跟随就比较简单。本文将虚拟坐标设置在了领航机器人的左下方和右下方，实现了三角编队；若想要实现其他队形编队，只需要修改对应的虚拟坐标即可。