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

山重水复疑无路@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

三角形编队

相关推荐
向阳逐梦18 小时前
基于STM32F4单片机实现ROS机器人主板
stm32·单片机·机器人
朽木成才1 天前
小程序快速实现大模型聊天机器人
小程序·机器人
聆思科技AI芯片1 天前
实操给桌面机器人加上超拟人音色
人工智能·机器人·大模型·aigc·多模态·智能音箱·语音交互
新加坡内哥谈技术2 天前
开源Genesis: 开创机器人研究的全新模拟平台
机器人·开源
野蛮的大西瓜2 天前
文心一言对接FreeSWITCH实现大模型呼叫中心
人工智能·机器人·自动化·音视频·实时音视频·文心一言·信息与通信
高克莱2 天前
【钉钉群聊机器人定时发送消息功能实现】
java·spring boot·机器人·调度任务
小俱的一步步2 天前
钉钉自定义机器人发送群消息（加签方式、http发送）
机器人·钉钉
三月七（爱看动漫的程序员）2 天前
Knowledge Graph Prompting for Multi-Document Question Answering
人工智能·gpt·学习·语言模型·自然语言处理·机器人·知识图谱
努力进修2 天前
【机器学习】当教育遇上机器学习：打破传统，开启因材施教新时代
人工智能·机器学习·机器人
高工智能汽车3 天前
踩准智能汽车+机器人两大风口，速腾聚创AI+机器人应用双线爆发
人工智能·机器人·汽车
热门推荐
01〔AI 绘画〕Stable Diffusion 之 解决绘制多人或面部很小的人物时面部崩坏问题 篇02ARM学习（31）编译器对overlay方式的支持03玄机平台应急响应—webshell查杀04DisplayPort AltMode On Type-c协议_Ver2.0(学习笔记)05校验 GPT-4 真实性的三个经典问题：快速区分 GPT-3.5 与 GPT-4，并提供免费测试网站06Docker 夺命连环 15 问076个主流的工业3D管道设计软件08基于大语言模型（LLM）的合成数据生成、策展和评估的综述09(欧拉)openEuler系统添加网卡文件配置流程、(欧拉)openEuler系统手动配置ipv6地址流程、(欧拉)openEuler系统网络管理说明10红米手机使用google play