文章目录
前言一、机器人编队前的准备二、配置仿真环境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_broadcaster1.cpp
touch tb3_tf_broadcaster2.cpp
touch tb3_tf_broadcaster3.cpp
touch tb3_tf_listener1.cpp
touch tb3_tf_listener2.cpp
touch tb3_tf_listener3.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的位置信息)
（3）编写机器人gazebo仿真环境 打开turtlebot3_simulations->turtlebot3_gazebo根据自己设计需要设置launch文件，这里为方便演示，我在multi_turtlebot3.launch文件的基础上进行修改，这里我只添加了三个机器人。 代码如下：
（4）运行launch文件进行测试 运行结果如下：
2.编写机器人编队.cpp文件
（1）编写广播文件代码 tb3_tf_broadcaster1
cd ~/catkin_ws/src/turtlebot3_teams_wang/src/
gedit tb3_tf_broadcaster1.cpp
插入如下代码：
#include
#include
#include
#include
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
gedit tb3_tf_broadcaster2.cpp
gedit tb3_tf_broadcaster3.cpp
插入如下代码：
#include
#include
#include
#include
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
#include
#include
#include
//#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 srv;
//add_turtle.call(srv);
// 创建发布tb3_1速度控制指令的发布者
ros::Publisher tb3_1_vel = node.advertise
// 创建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;
};
tb3_tf_listener2.cpp插入如下代码：
#include
#include
#include
#include
//#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 srv;
//add_turtle.call(srv);
// 创建发布tb3_1速度控制指令的发布者
ros::Publisher tb3_2_vel = node.advertise
// 创建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的名称需要和添加的小车机器人名称一一对应。 代码如下：
args="/tb3_0" name="robot_0_tf_broadcaster" /> args="/tb3_1" name="robot_1_tf_broadcaster" /> args="/tb3_2" name="robot_2_tf_broadcaster" /> name="follower1" /> name="follower2" /> （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 三角形编队 好文推荐 大家都在找： 机器人：机器人酷跑3 多机器人编队：多机器人编队控制方法
您阅读本篇文章共花了：
发表评论