多机器人三角形编队的实现

山重水复疑无路@2023-10-16 15:46

文章目录

前言

前阵子一直想要实现多机器人编队，找到了很多开源的编队代码，经过好几天的思索，终于实现了在gazebo环境中的TB3三角形机器人编队。

一、机器人编队前的准备

本次实现的多机器人三角形编队是在之前配置完成的单个TB3机器人基础上实现的，如果想要配置单个机器人可以参考这篇文章：双系统ubuntu20.04(neotic版本)从0实现Gazebo仿真slam建图

（1）创建工作空间：mkdir -p ~/catkin_ws/src

（2）把前面做好的单个机器人导航键图的功能包拷贝到src中。

可参考文章：ROS如何将拷贝的功能包成功运行在自己的工作空间中

（3）创建多机器人编队的功能包：

cpp 复制代码 
catkin_create_pkg turtlebot3_teams_wang roscpp rospy tf turtlesim

（4）新建广播以及接收广播的对应的.cpp文件

cpp 复制代码 
cd ~/catkin_ws/src/turtlebot3_teams_wang/src/
touch tb3_tf_broadcaster.cpp
touch tb3_tf_listener1.cpp
touch tb3_tf_listener2.cpp
touch tb3_tf_listener3.cpp
touch tb3_tf_listener4.cpp

（5）创建launch启动文件

cpp 复制代码 
cd ~/catkin_ws/src/turtlebot3_teams_wang/launch
touch turtlebot3_teams_follow_zhou.launch

二、配置仿真环境

（1）打开驱相应urdf.xacro模型（burger,waffle,waffle_pi都行）

本文选取waffle机器人模型

（2）插入以下代码增加话题订阅(订阅base_pose_ground_truth话题，gazebo可获取机器人相对与world的位置信息)

cpp 复制代码 
  <gazebo>
    <plugin name="base_waffle_controller" filename="libgazebo_ros_p3d.so">
      <alwaysOn>true</alwaysOn>
      <updateRate>50.0</updateRate>
      <bodyName>base_footprint</bodyName>
      <topicName>base_pose_ground_truth</topicName>
      <gaussianNoise>0.01</gaussianNoise>
      <frameName>world</frameName>
      <xyzOffsets>0 0 0</xyzOffsets>
      <rpyOffsets>0 0 0</rpyOffsets>
    </plugin>
  </gazebo>

（3）编写机器人gazebo仿真环境

打开turtlebot3_simulations->turtlebot3_gazebo根据自己设计需要设置launch文件，这里为方便演示，我在multi_turtlebot3.launch文件的基础上进行修改，这里我只添加了三个机器人。
代码如下：

cpp 复制代码 
<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=" 1.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=" 0.0"/>
  <arg name="second_tb3_y_pos" default="-1.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=" 0.0"/>
  <arg name="third_tb3_y_pos" default=" 1.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/empty.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>

（4）运行launch文件进行测试

运行结果如下：

2.编写机器人编队.cpp文件

（1）编写广播文件代码

tb3_tf_broadcaster1

cpp 复制代码 
cd ~/catkin_ws/src/turtlebot3_teams_wang/src/
gedit tb3_tf_broadcaster1.cpp

插入如下代码：

cpp 复制代码 
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>
#include <nav_msgs/Odometry.h>
std::string turtle_name;
std::string robot_name;

void poseCallback(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((msg->pose.pose.position.x)-0.5, (msg->pose.pose.position.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_0"));
	transform1.setOrigin( tf::Vector3((msg->pose.pose.position.x)-0.5, (msg->pose.pose.position.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"));
}


int main(int argc, char** argv)
{
    // 初始化ROS节点
	ros::init(argc, argv, "my_tf_broadcaster");

	// 输入参数作为海龟的名字
	if (argc != 2)
	{
		ROS_ERROR("need turtle name as argument"); 
		return -1;
	}
	robot_name = argv[1];
	// 订阅海龟的位姿话题
	ros::NodeHandle node;
	ros::Subscriber sub = node.subscribe(robot_name+"/base_pose_ground_truth", 10, &poseCallback);
	//ros::Subscriber sub = node.subscribe(robot_name+"/odom", 10, &poseCallback);
    	// 循环等待回调函数
	ros::spin();
	return 0;
};

tb3_tf_broadcaster1与tb3_tf_broadcaster2

cpp 复制代码 
gedit tb3_tf_broadcaster2.cpp
gedit tb3_tf_broadcaster3.cpp

插入如下代码：

cpp 复制代码 
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>
#include <nav_msgs/Odometry.h>
std::string turtle_name;
std::string robot_name;
void poseCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
	// 创建tf的广播器
	static tf::TransformBroadcaster br;

	// 初始化tf数据
	tf::Transform transform;
	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", robot_name));
}

int main(int argc, char** argv)
{
    // 初始化ROS节点
	ros::init(argc, argv, "my_tf_broadcaster");

	// 输入参数作为海龟的名字
	if (argc != 2)
	{
		ROS_ERROR("need turtle name as argument"); 
		return -1;
	}
	robot_name = argv[1];
	// 订阅海龟的位姿话题
	ros::NodeHandle node;
	ros::Subscriber sub = node.subscribe(robot_name+"/base_pose_ground_truth", 10, &poseCallback);
	//ros::Subscriber sub = node.subscribe(robot_name+"/odom", 10, &poseCallback);
    	// 循环等待回调函数
	ros::spin();
	return 0;
};

（2）编写tf接收器文件代码

1、在对应路径下打开.cpp文件

cpp 复制代码 
cd ~/catkin_ws/src/turtlebot3_teams_wang/src/
gedit tb3_tf_listener1.cpp
gedit tb3_tf_listener2.cpp

tb3_tf_listener1.cpp插入如下代码：

cpp 复制代码 
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
//#include "sensor_msgs/LaserScan.h"

int main(int argc, char** argv)
{
	// 初始化ROS节点
	ros::init(argc, argv, "my_tf_listener");

    // 创建节点句柄
	ros::NodeHandle node;

	// 请求产生turtle2
	//ros::service::waitForService("/spawn");
	//ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");
	//turtlesim::Spawn srv;
	//add_turtle.call(srv);

	// 创建发布tb3_1速度控制指令的发布者
	ros::Publisher tb3_1_vel = node.advertise<geometry_msgs::Twist>("/tb3_1/cmd_vel", 10);

	// 创建tf的监听器
	tf::TransformListener listener;

	ros::Rate rate(10.0);
	while (node.ok())
	{
		// 获取turtle1与turtle2坐标系之间的tf数据
		tf::StampedTransform transformfl;
		tf::StampedTransform transformlf;
		try
		{
		        listener.waitForTransform("/tb3_1", "/virtual_0", ros::Time(0), ros::Duration(3.0));
			listener.lookupTransform("/tb3_1", "/virtual_0", ros::Time(0), transformfl);
			
		}
		catch (tf::TransformException &ex) 
		{
			ROS_ERROR("%s",ex.what());
			ros::Duration(1.0).sleep();
			continue;
		}
		try#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
//#include "sensor_msgs/LaserScan.h"

int main(int argc, char** argv)
{
	// 初始化ROS节点
	ros::init(argc, argv, "my_tf_listener");

    // 创建节点句柄
	ros::NodeHandle node;

	// 请求产生turtle2
	//ros::service::waitForService("/spawn");
	//ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");
	//turtlesim::Spawn srv;
	//add_turtle.call(srv);

	// 创建发布tb3_1速度控制指令的发布者
	ros::Publisher tb3_1_vel = node.advertise<geometry_msgs::Twist>("/tb3_1/cmd_vel", 10);

	// 创建tf的监听器
	tf::TransformListener listener;

	ros::Rate rate(10.0);
	while (node.ok())
	{
		// 获取turtle1与turtle2坐标系之间的tf数据
		tf::StampedTransform transformfl;
		tf::StampedTransform transformlf;
		try
		{
		        listener.waitForTransform("/tb3_1", "/virtual_0", ros::Time(0), ros::Duration(3.0));
			listener.lookupTransform("/tb3_1", "/virtual_0", ros::Time(0), transformfl);
			
		}
		catch (tf::TransformException &ex) 
		{
			ROS_ERROR("%s",ex.what());
			ros::Duration(1.0).sleep();
			continue;
		}
		try
		{
		        listener.waitForTransform("/virtual_0", "/tb3_1", ros::Time(0), ros::Duration(3.0));
			listener.lookupTransform("/virtual_0", "/tb3_1", ros::Time(0), transformlf);
			
		}
		catch (tf::TransformException &ex) 
		{
			ROS_ERROR("%s",ex.what());
			ros::Duration(1.0).sleep();
			continue;
		}

		// 根据tb3_0与tb3_1坐标系之间的位置关系，发布turtle2的速度控制指令
		geometry_msgs::Twist vel_msg;
		vel_msg.angular.z = 4.0 * atan2(transformfl.getOrigin().y(),
				                        transformfl.getOrigin().x());
		vel_msg.linear.x = 0.5 * sqrt(pow(transformfl.getOrigin().x(), 2) +
				                      pow(transformfl.getOrigin().y(), 2));
		tb3_1_vel.publish(vel_msg);

		rate.sleep();
	}
	return 0;
};

		{
		        listener.waitForTransform("/virtual_0", "/tb3_1", ros::Time(0), ros::Duration(3.0));
			listener.lookupTransform("/virtual_0", "/tb3_1", ros::Time(0), transformlf);
			
		}
		catch (tf::TransformException &ex) 
		{
			ROS_ERROR("%s",ex.what());
			ros::Duration(1.0).sleep();
			continue;
		}

		// 根据tb3_0与tb3_1坐标系之间的位置关系，发布turtle2的速度控制指令
		geometry_msgs::Twist vel_msg;
		vel_msg.angular.z = 4.0 * atan2(transformfl.getOrigin().y(),
				                        transformfl.getOrigin().x());
		vel_msg.linear.x = 0.5 * sqrt(pow(transformfl.getOrigin().x(), 2) +
				                      pow(transformfl.getOrigin().y(), 2));
		tb3_1_vel.publish(vel_msg);

		rate.sleep();
	}
	return 0;
};

tb3_tf_listener2.cpp插入如下代码：

cpp 复制代码 
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
//#include "sensor_msgs/LaserScan.h"

int main(int argc, char** argv)
{
	// 初始化ROS节点
	ros::init(argc, argv, "my_tf_listener");

    // 创建节点句柄
	ros::NodeHandle node;

	// 请求产生turtle2
	//ros::service::waitForService("/spawn");
	//ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");
	//turtlesim::Spawn srv;
	//add_turtle.call(srv);

	// 创建发布tb3_1速度控制指令的发布者
	ros::Publisher tb3_2_vel = node.advertise<geometry_msgs::Twist>("/tb3_2/cmd_vel", 10);

	// 创建tf的监听器
	tf::TransformListener listener;

	ros::Rate rate(10.0);
	while (node.ok())
	{
		// 获取turtle1与turtle2坐标系之间的tf数据
		tf::StampedTransform transformfl;
		tf::StampedTransform transformlf;
		try
		{
		        listener.waitForTransform("/tb3_2", "/virtual_1", ros::Time(0), ros::Duration(3.0));
			listener.lookupTransform("/tb3_2", "/virtual_1", ros::Time(0), transformfl);
			
		}
		catch (tf::TransformException &ex) 
		{
			ROS_ERROR("%s",ex.what());
			ros::Duration(1.0).sleep();
			continue;
		}
		try
		{
		        listener.waitForTransform("/virtual_1", "/tb3_2", ros::Time(0), ros::Duration(3.0));
			listener.lookupTransform("/virtual_1", "/tb3_2", ros::Time(0), transformlf);
			
		}
		catch (tf::TransformException &ex) 
		{
			ROS_ERROR("%s",ex.what());
			ros::Duration(1.0).sleep();
			continue;
		}

		// 根据tb3_0与tb3_1坐标系之间的位置关系，发布turtle2的速度控制指令
		geometry_msgs::Twist vel_msg;
		vel_msg.angular.z = 4.0 * atan2(transformfl.getOrigin().y(),
				                        transformfl.getOrigin().x());
		vel_msg.linear.x = 0.5 * sqrt(pow(transformfl.getOrigin().x(), 2) +
				                      pow(transformfl.getOrigin().y(), 2));
		tb3_2_vel.publish(vel_msg);

		rate.sleep();
	}
	return 0;
};

（3）在对应路径下编辑launch文件

cpp 复制代码 
gedit turtlebot3_teams_follow_wang.launch

注意：和.cpp文件名对应

注意：args的名称需要和添加的小车机器人名称一一对应。

代码如下：

cpp 复制代码 
 <launch>
    <node pkg="turtlebot3_teams_zhou" type="tb3_tf_broadcaster1"
          args="/tb3_0" name="robot_0_tf_broadcaster" />
    <node pkg="turtlebot3_teams_zhou" type="tb3_tf_broadcaster2"
          args="/tb3_1" name="robot_1_tf_broadcaster" />
    <node pkg="turtlebot3_teams_zhou" type="tb3_tf_broadcaster3"
          args="/tb3_2" name="robot_2_tf_broadcaster" />

   
    <node pkg="turtlebot3_teams_zhou" type="tb3_tf_listener1"
          name="follower1" />
    <node pkg="turtlebot3_teams_zhou" type="tb3_tf_listener2"
          name="follower2" />

  </launch>

（4）编译工作环境

1、在turtlebot3_teams_wang的功能包下打开CMakeLists.txt文件，在Build中插入相应代码

注意：命令需要和.cpp文件名对应

cpp 复制代码 
add_executable(tb3_tf_broadcaster1 src/tb3_tf_broadcaster1.cpp)
target_link_libraries(tb3_tf_broadcaster1 ${catkin_LIBRARIES})

add_executable(tb3_tf_broadcaster2 src/tb3_tf_broadcaster2.cpp)
target_link_libraries(tb3_tf_broadcaster2 ${catkin_LIBRARIES})

add_executable(tb3_tf_broadcaster3 src/tb3_tf_broadcaster3.cpp)
target_link_libraries(tb3_tf_broadcaster3 ${catkin_LIBRARIES})

add_executable(tb3_tf_listener1 src/tb3_tf_listener1.cpp)
target_link_libraries(tb3_tf_listener1  ${catkin_LIBRARIES})

add_executable(tb3_tf_listener2 src/tb3_tf_listener2.cpp)
target_link_libraries(tb3_tf_listener2  ${catkin_LIBRARIES})

三、三角形编队测试

（1）在测试之前先编译下工作空间

cpp 复制代码 
cd ~/catkin_ws
catkin_make

（2）运行机器人仿真环境

cpp 复制代码 
roslaunch turtlebot3_gazebo multi_turtlebot3.launch

（3）启动编队程序

cpp 复制代码 
roslaunch turtlebot3_teams_zhou turtlebot3_teams_follow_wang.launch

（4）控制tb3_0小车进行运动

cpp 复制代码 
ROS_NAMESPACE=tb3_0 rosrun turtlebot3_teleop turtlebot3_teleop_key

三角形编队

相关推荐
会的东西有点杂12 小时前
ROS 2和ROS 1的比较
机器人
酉鬼女又兒12 小时前
零基础入门 DeepSeek V4 Pro API 开发：从环境搭建、消息格式规范到翻译函数实战、少样本提示、多轮对话聊天机器人与常见报错全流程详解指南
大数据·网络·数据库·人工智能·macos·机器人·github
小顿的企业观察14 小时前
从人形机器人到AI销售智能体，AI正在重塑中企出海的底层逻辑
大数据·运维·人工智能·机器人·产品运营·制造
沫儿笙17 小时前
安川弧焊机器人混合气动态调节技术
人工智能·机器人
学术小白人19 小时前
第二届智能制造、机器人与自动化国际学术会议（IMRA 2026）
运维·人工智能·机器人·自动化·制造
人生百态，人生如梦20 小时前
情感交互仿生人从技术到落地构想2——技术交流贴（2026.7）
人工智能·机器人·人机交互·交互
TMT星球1 天前
机器人管家系统上线！傅利叶携多款康养陪伴新品方案亮相WAIC 2026
机器人
咕噜咕噜啦啦1 天前
ROS2自主导航
机器人·ros·gazebo
田里的水稻1 天前
EP_局域网传输音视频的主流方案
运维·人工智能·机器人·音视频
蜜桃味女焊匠人1 天前
2026焊接保护气体降耗最优改造方案
人工智能·经验分享·其他·机器人
热门推荐
012026年7月AI圈大地震：GPT-5.6被政府限制、Claude入驻Slack、Anthropic自研芯片02GitHub 镜像站点03AI科技热点日报 | 2026年07月01日04幻兽帕鲁 - 服务器管理员权限与 GM 命令完全指南05如何新建文件夹？ 电脑新建文件夹的4种方法06AI 编程 IDE 全景解析 2026：Agent 全面接管开发链路072026年AI技术突破与产业落地全景：从GPT-5到多模态智能体的新纪元082026 国产 AI 大模型横评：DeepSeek、通义千问、Kimi、文心一言、星火、豆包谁更能打？09国内可直接用、免费额度/永久免费的大模型API清单（含 SiliconFlow、火山、阿里、智谱、百度、Kimi、DeepSeek、DMXAPI 等）10几个好用的ip纯净度检测网站