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

2023-10-14 16:28
文章标签 实现 机器人 三角形 编队

本文主要是介绍多机器人三角形编队的实现，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

文章目录

  • 前言
  • 一、机器人编队前的准备
  • 二、配置仿真环境
    • 2.编写机器人编队.cpp文件
  • 三、三角形编队测试

前言

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

一、机器人编队前的准备

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

（1）创建工作空间：mkdir -p ~/catkin_ws/src
（2）把前面做好的单个机器人导航键图的功能包拷贝到src中。
可参考文章：ROS如何将拷贝的功能包成功运行在自己的工作空间中
（3）创建多机器人编队的功能包：

catkin_create_pkg turtlebot3_teams_wang roscpp rospy tf turtlesim

（4）新建广播以及接收广播的对应的.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启动文件

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的位置信息)

  <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文件的基础上进行修改，这里我只添加了三个机器人。
在这里插入图片描述代码如下：

<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

cd ~/catkin_ws/src/turtlebot3_teams_wang/src/
gedit tb3_tf_broadcaster1.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,yzhitransform0.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,yzhitransform1.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

gedit tb3_tf_broadcaster2.cpp
gedit tb3_tf_broadcaster3.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文件

cd ~/catkin_ws/src/turtlebot3_teams_wang/src/
gedit tb3_tf_listener1.cpp
gedit tb3_tf_listener2.cpp

tb3_tf_listener1.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插入如下代码：

#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文件

gedit turtlebot3_teams_follow_wang.launch

注意：和.cpp文件名对应
注意：args的名称需要和添加的小车机器人名称一一对应。
代码如下：

 <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文件名对应

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）在测试之前先编译下工作空间

cd ~/catkin_ws
catkin_make

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

roslaunch turtlebot3_gazebo multi_turtlebot3.launch

（3）启动编队程序

roslaunch turtlebot3_teams_zhou turtlebot3_teams_follow_wang.launch

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

ROS_NAMESPACE=tb3_0 rosrun turtlebot3_teleop turtlebot3_teleop_key

三角形编队

这篇关于多机器人三角形编队的实现的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/211700。

相关文章

Python位移操作和位运算的实现示例

Python位移操作和位运算的实现示例

《Python位移操作和位运算的实现示例》本文主要介绍了Python位移操作和位运算的实现示例,文中通过示例代码介绍的非常详细,对大家的学习或者工作具有一定的参考学习价值,需要的朋友们下面随着小编来一... 目录1. 位移操作1.1 左移操作 (<<)1.2 右移操作 (>>)注意事项：2. 位运算2.1
阅读更多...
如何在 Spring Boot 中实现 FreeMarker 模板

如何在 Spring Boot 中实现 FreeMarker 模板

《如何在SpringBoot中实现FreeMarker模板》FreeMarker是一种功能强大、轻量级的模板引擎,用于在Java应用中生成动态文本输出（如HTML、XML、邮件内容等）,本文... 目录什么是 FreeMarker 模板？在 Spring Boot 中实现 FreeMarker 模板1. 环
阅读更多...
Qt实现网络数据解析的方法总结

Qt实现网络数据解析的方法总结

《Qt实现网络数据解析的方法总结》在Qt中解析网络数据通常涉及接收原始字节流,并将其转换为有意义的应用层数据,这篇文章为大家介绍了详细步骤和示例,感兴趣的小伙伴可以了解下... 目录1. 网络数据接收2. 缓冲区管理（处理粘包/拆包）3. 常见数据格式解析3.1 jsON解析3.2 XML解析3.3 自定义
阅读更多...
SpringMVC 通过ajax 前后端数据交互的实现方法

SpringMVC 通过ajax 前后端数据交互的实现方法

《SpringMVC通过ajax前后端数据交互的实现方法》：本文主要介绍SpringMVC通过ajax前后端数据交互的实现方法,本文给大家介绍的非常详细,对大家的学习或工作具有一定的参考借鉴价... 在前端的开发过程中，经常在html页面通过AJAX进行前后端数据的交互，SpringMVC的controll
阅读更多...
Spring Security自定义身份认证的实现方法

Spring Security自定义身份认证的实现方法

《SpringSecurity自定义身份认证的实现方法》：本文主要介绍SpringSecurity自定义身份认证的实现方法,下面对SpringSecurity的这三种自定义身份认证进行详细讲解,... 目录1.内存身份认证（1）创建配置类（2）验证内存身份认证2.JDBC身份认证（1）数据准备 （2）配置依
阅读更多...
利用python实现对excel文件进行加密

利用python实现对excel文件进行加密

《利用python实现对excel文件进行加密》由于文件内容的私密性,需要对Excel文件进行加密,保护文件以免给第三方看到,本文将以Python语言为例,和大家讲讲如何对Excel文件进行加密,感兴... 目录前言方法一：使用pywin32库（仅限Windows）方法二：使用msoffcrypto-too
阅读更多...
C#使用StackExchange.Redis实现分布式锁的两种方式介绍

C#使用StackExchange.Redis实现分布式锁的两种方式介绍

《C#使用StackExchange.Redis实现分布式锁的两种方式介绍》分布式锁在集群的架构中发挥着重要的作用,：本文主要介绍C#使用StackExchange.Redis实现分布式锁的... 目录自定义分布式锁获取锁释放锁自动续期StackExchange.Redis分布式锁获取锁释放锁自动续期分布式
阅读更多...
springboot使用Scheduling实现动态增删启停定时任务教程

springboot使用Scheduling实现动态增删启停定时任务教程

《springboot使用Scheduling实现动态增删启停定时任务教程》：本文主要介绍springboot使用Scheduling实现动态增删启停定时任务教程,具有很好的参考价值,希望对大家有... 目录1、配置定时任务需要的线程池2、创建ScheduledFuture的包装类3、注册定时任务，增加、删
阅读更多...
SpringBoot整合mybatisPlus实现批量插入并获取ID详解

SpringBoot整合mybatisPlus实现批量插入并获取ID详解

《SpringBoot整合mybatisPlus实现批量插入并获取ID详解》这篇文章主要为大家详细介绍了SpringBoot如何整合mybatisPlus实现批量插入并获取ID,文中的示例代码讲解详细... 目录【1】saveBATch(一万条数据总耗时：2478ms)【2】集合方式foreach(一万条数
阅读更多...
使用Python实现矢量路径的压缩、解压与可视化

使用Python实现矢量路径的压缩、解压与可视化

《使用Python实现矢量路径的压缩、解压与可视化》在图形设计和Web开发中,矢量路径数据的高效存储与传输至关重要,本文将通过一个Python示例,展示如何将复杂的矢量路径命令序列压缩为JSON格式,... 目录引言核心功能概述1. 路径命令解析2. 路径数据压缩3. 路径数据解压4. 可视化代码实现详解1
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2