文章目录

前言一、机器人编队前的准备二、配置仿真环境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的位置信息)

true

50.0

base_footprint

base_pose_ground_truth

0.01

world

0 0 0

0 0 0

(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("/spawn");

//turtlesim::Spawn srv;

//add_turtle.call(srv);

// 创建发布tb3_1速度控制指令的发布者

ros::Publisher tb3_1_vel = node.advertise("/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;

};

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("/spawn");

//turtlesim::Spawn srv;

//add_turtle.call(srv);

// 创建发布tb3_1速度控制指令的发布者

ros::Publisher tb3_2_vel = node.advertise("/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的名称需要和添加的小车机器人名称一一对应。 代码如下:

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

三角形编队

好文推荐

评论可见,请评论后查看内容,谢谢!!!
 您阅读本篇文章共花了: