在gazebo中5个机器人进行一字型编队
（1）在古月居的ros入门18讲基础上完成一字型编队
（2）采用单节点、多线程的方式完成一字型编队
前言
前文已经阐述了如何添加多个机器人以及gazebo如何添加环境模型，本文以tf通信方式通过broadercaster发布坐标，最后使跟随者收听相对应前一个机器人的坐标，在进行跟随，这样就可以完成一字型编队
一、在古月居的ros入门18讲基础上完成一字型编队
(1)打开驱相应模型的.gazebo.xacroo文件插入以下插件(以已有路径为例)
gedit ~/catkin_ws/src/turtlebot3/turtlebot3_description/urdf/turtlebot3_burger.gazebo.xacro
(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)创建并编写.cpp文件
1)新建功能包、新建广播以及接收广播的对应的.cpp文件
cd ~/catkin_ws/src
catkin_create_pkg turtlebot3_teams_zhou roscpp rospy tf turtlesim
cd ~/catkin_ws/src/turtlebot3_teams_zhou/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
2）编写广播文件代码
1、在对应路径下打开.cpp文件
cd ~/catkin_ws/src/turtlebot3_teams_zhou/src/
gedit tb3_tf_broadcaster.cpp
2、插入以下代码
#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;
};
3）编写tf接收器文件代码
1、在对应路径下打开.cpp文件
cd ~/catkin_ws/src/turtlebot3_teams_zhou/src/
gedit tb3_tf_listener1.cpp
2、插入相应代码
tb3_tf_listener2.cpp、tb3_tf_listener3.cpp和tb3_tf_listener4.cpp同理，需将对应的/tb3_1、/tb3_1、/tb3_2、/tb3_3和/tb3_4进行替换
#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", "/tb3_0", ros::Time(0), ros::Duration(3.0));
listener.lookupTransform("/tb3_1", "/tb3_0", ros::Time(0), transformfl);
}
catch (tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
try
{
listener.waitForTransform("/tb3_0", "/tb3_1", ros::Time(0), ros::Duration(3.0));
listener.lookupTransform("/tb3_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;
};
4)编写launch启动文件
1、在对应路径下创建launch文件
cd ~/catkin_ws/src/turtlebot3_teams_zhou/launch
touch turtlebot3_teams_follow_zhou.launch
2、在对应路径下编辑launch文件
gedit turtlebot3_teams_follow_zhou.launch
3、插入相应代码
注意：和.cpp文件名对应
注意：args的名称需要和添加的小车机器人名称一一对应。
<launch>
<node pkg="turtlebot3_teams_zhou" type="tb3_tf_broadcaster"
args="/tb3_0" name="robot_0_tf_broadcaster" />
<node pkg="turtlebot3_teams_zhou" type="tb3_tf_broadcaster"
args="/tb3_1" name="robot_1_tf_broadcaster" />
<node pkg="turtlebot3_teams_zhou" type="tb3_tf_broadcaster"
args="/tb3_2" name="robot_2_tf_broadcaster" />
<node pkg="turtlebot3_teams_zhou" type="tb3_tf_broadcaster"
args="/tb3_3" name="robot_3_tf_broadcaster" />
<node pkg="turtlebot3_teams_zhou" type="tb3_tf_broadcaster"
args="/tb3_4" name="robot_4_tf_broadcaster" />
<node pkg="turtlebot3_teams_zhou" type="tb3_tf_listener1"
name="follower1" />
<node pkg="turtlebot3_teams_zhou" type="tb3_tf_listener2"
name="follower2" />
<node pkg="turtlebot3_teams_zhou" type="tb3_tf_listener3"
name="follower3" />
<node pkg="turtlebot3_teams_zhou" type="tb3_tf_listener4"
name="follower4" />
</launch>
5）编译工作环境
1、在对应路径下打开CMakeLists.txt文件，在Build中插入相应代码
注意：命令需要和.cpp文件名对应
add_executable(tb3_tf_broadcaster src/tb3_tf_broadcaster.cpp)
target_link_libraries(tb3_tf_broadcaster ${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})
add_executable(tb3_tf_listener3 src/tb3_tf_listener3.cpp)
target_link_libraries(tb3_tf_listener3 ${catkin_LIBRARIES})
add_executable(tb3_tf_listener4 src/tb3_tf_listener4.cpp)
target_link_libraries(tb3_tf_listener4 ${catkin_LIBRARIES})
(4)编译环境
cd ~/catkin_ws
catkin_make
(5)运行仿真
##新终端
roslaunch turtlebot3_gazebo multi_turtlebot3.launch
##新终端
roslaunch turtlebot3_teams_zhou turtlebot3_teams_follow_zhou.launch
##新终端
ROS_NAMESPACE=tb3_0 rosrun turtlebot3_teleop turtlebot3_teleop_key
一字型
二、采用单节点、多线程的方式完成一字型编队
第一种方式比较繁琐，接下来采用单节点、多线程的方式完成一字型编队，并且可以解决死区问题（碰撞）
(1)在原有功能包种添加对应的.cpp文件
cd ~/catkin_ws/src/turtlebot3_teams_zhou/src/
touch fifth_subscriber-siqu.cpp
(2）编写.cpp代码，在对应路径下打开.cpp文件，插入以下代码
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/String.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <tf/transform_broadcaster.h>
class turtlebot3_broadcaster
{
public:
turtlebot3_broadcaster()
{
//count = 0;
//Topic you want to publish
//Topic1 you want to subscribe
//sub1 = node.subscribe("cmd_vel", 1, &SubscribeAndPublish::callback1, this);
sub0 = node.subscribe("/tb3_0/base_pose_ground_truth", 10, &turtlebot3_broadcaster::poseCallback0,this);
sub1 = node.subscribe("/tb3_1/base_pose_ground_truth", 10, &turtlebot3_broadcaster::poseCallback1,this);
sub2 = node.subscribe("/tb3_2/base_pose_ground_truth", 10, &turtlebot3_broadcaster::poseCallback2,this);
sub3 = node.subscribe("/tb3_3/base_pose_ground_truth", 10, &turtlebot3_broadcaster::poseCallback3,this);
sub4 = node.subscribe("/tb3_4/base_pose_ground_truth", 10, &turtlebot3_broadcaster::poseCallback4,this);
}
void poseCallback0(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", "/tb3_0"));
}
void poseCallback1(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", "/tb3_1"));
}
void poseCallback2(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", "/tb3_2"));
}
void poseCallback3(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", "/tb3_3"));
}
void poseCallback4(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", "/tb3_4"));
}
private:
ros::NodeHandle node;
ros::Subscriber sub0;
ros::Subscriber sub1;
ros::Subscriber sub2;
ros::Subscriber sub3;
ros::Subscriber sub4;
std_msgs::String output;
int count;
};//End of class SubscribeAndPublish
//坐标关系转化为线速度与角速度，解决死区问题
void StampedTransformToTwist(tf::StampedTransform &transform,geometry_msgs::Twist &vel_msg)
{
float distance;
vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(), transform.getOrigin().x());
distance = sqrt(pow(transform.getOrigin().x(), 2) + pow(transform.getOrigin().y(), 2));//获得两小车之间的距离
vel_msg.linear.x = 50*pow(distance-0.4, 3);
if(0.3<distance&&distance<0.5) vel_msg.linear.x = 0;
}
void turtlebot3_listener()
{
// 创建节点句柄
ros::NodeHandle node;
// 创建发布tb3_x速度控制指令的发布者
ros::Publisher tb3_0_vel = node.advertise<geometry_msgs::Twist>("/tb3_0/cmd_vel", 10);
ros::Publisher tb3_1_vel = node.advertise<geometry_msgs::Twist>("/tb3_1/cmd_vel", 10);
ros::Publisher tb3_2_vel = node.advertise<geometry_msgs::Twist>("/tb3_2/cmd_vel", 10);
ros::Publisher tb3_3_vel = node.advertise<geometry_msgs::Twist>("/tb3_3/cmd_vel", 10);
ros::Publisher tb3_4_vel = node.advertise<geometry_msgs::Twist>("/tb3_4/cmd_vel", 10);
// 创建tf的监听器
tf::TransformListener listener;
ros::Rate rate(10.0);
while (node.ok())
{
// 获取turtle1与turtle2坐标系之间的tf数据
tf::StampedTransform transformfl1;
tf::StampedTransform transformlf1;
tf::StampedTransform transformfl2;
tf::StampedTransform transformlf2;
tf::StampedTransform transformfl3;
tf::StampedTransform transformlf3;
tf::StampedTransform transformfl4;
tf::StampedTransform transformlf4;
try
{
listener.waitForTransform("/tb3_1", "/tb3_0", ros::Time(0), ros::Duration(3.0));
listener.lookupTransform("/tb3_1", "/tb3_0", ros::Time(0), transformfl1);
listener.waitForTransform("/tb3_2", "/tb3_1", ros::Time(0), ros::Duration(3.0));
listener.lookupTransform("/tb3_2", "/tb3_1", ros::Time(0), transformfl2);
listener.waitForTransform("/tb3_3", "/tb3_2", ros::Time(0), ros::Duration(3.0));
listener.lookupTransform("/tb3_3", "/tb3_2", ros::Time(0), transformfl3);
listener.waitForTransform("/tb3_4", "/tb3_3", ros::Time(0), ros::Duration(3.0));
listener.lookupTransform("/tb3_4", "/tb3_3", ros::Time(0), transformfl4);
//ROS_INFO("data:%f,%f",transformfl1.getOrigin().x(),transformfl1.getOrigin().y());
}
catch (tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
try
{
listener.waitForTransform("/tb3_0", "/tb3_1", ros::Time(0), ros::Duration(3.0));
listener.lookupTransform("/tb3_0", "/tb3_1", ros::Time(0), transformlf1);
listener.waitForTransform("/tb3_1", "/tb3_4", ros::Time(0), ros::Duration(3.0));
listener.lookupTransform("/tb3_1", "/tb3_4", ros::Time(0), transformlf2);
listener.waitForTransform("/tb3_2", "/tb3_4", ros::Time(0), ros::Duration(3.0));
listener.lookupTransform("/tb3_2", "/tb3_4", ros::Time(0), transformlf3);
listener.waitForTransform("/tb3_3", "/tb3_4", ros::Time(0), ros::Duration(3.0));
listener.lookupTransform("/tb3_3", "/tb3_4", ros::Time(0), transformlf4);
}
catch (tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
// 根据机器人坐标系之间的位置关系，发布速度控制指令
geometry_msgs::Twist vel_msg0;
geometry_msgs::Twist vel_msg1;
geometry_msgs::Twist vel_msg2;
geometry_msgs::Twist vel_msg3;
geometry_msgs::Twist vel_msg4;
vel_msg0.linear.x = 0.5;
vel_msg0.angular.z = 0.2;
// vel_msg1.angular.z = 4.0 * atan2(transformfl1.getOrigin().y(),
// transformfl1.getOrigin().x());
// vel_msg1.linear.x = 0.5 * sqrt(pow(transformfl1.getOrigin().x(), 2) +
// pow(transformfl1.getOrigin().y(), 2));
// vel_msg2.angular.z = 4.0 * atan2(transformfl2.getOrigin().y(),
// transformfl2.getOrigin().x());
// vel_msg2.linear.x = 0.5 * sqrt(pow(transformfl2.getOrigin().x(), 2) +
// pow(transformfl2.getOrigin().y(), 2));
// vel_msg3.angular.z = 4.0 * atan2(transformfl3.getOrigin().y(),
// transformfl3.getOrigin().x());
// vel_msg3.linear.x = 0.5 * sqrt(pow(transformfl3.getOrigin().x(), 2) +
// pow(transformfl3.getOrigin().y(), 2));
// vel_msg4.angular.z = 4.0 * atan2(transformfl4.getOrigin().y(),
// transformfl4.getOrigin().x());
// vel_msg4.linear.x = 0.5 * sqrt(pow(transformfl4.getOrigin().x(), 2) +
// pow(transformfl4.getOrigin().y(), 2));
//将坐标关系分别转化为速度
StampedTransformToTwist(transformfl1,vel_msg1);
StampedTransformToTwist(transformfl2,vel_msg2);
StampedTransformToTwist(transformfl3,vel_msg3);
StampedTransformToTwist(transformfl4,vel_msg4);
//tb3_0_vel.publish(vel_msg0);
tb3_1_vel.publish(vel_msg1);
tb3_2_vel.publish(vel_msg2);
tb3_3_vel.publish(vel_msg3);
tb3_4_vel.publish(vel_msg4);
rate.sleep();
}
}
int main(int argc, char** argv)
{
// 初始化ROS节点
ros::init(argc, argv, "fifth_subscriber_1");
// 创建节点句柄
//ros::NodeHandle node;
//多线程开启
ros::MultiThreadedSpinner mult(6);//multithreading
// 设置发布频率
//ros::Rate rate(10);
//SubscribeAndPublish Node_Test;
//创建tf的广播器
turtlebot3_broadcaster turtlebot3_broadcaster_five;
//创建C++新进程进行监听
boost::thread server(turtlebot3_listener);
//等待回调
//ros::spin();
mult.spin();
//ros::spin(mult);
return 0;
};
3）编译工作环境
1、在对应路径下打开CMakeLists.txt文件，在Build中插入相应代码
注意：命令需要和.cpp文件名对应
add_executable(fifth_subscriber-siqu src/fifth_subscriber-siqu.cpp)
target_link_libraries(fifth_subscriber-siqu ${catkin_LIBRARIES})
(3)运行仿真
##新终端
roslaunch turtlebot3_gazebo multi_turtlebot3.launch
##新终端
rosrun turtlebot3_teams_zhou fifth_subscriber-siqu
##新终端
ROS_NAMESPACE=tb3_0 rosrun turtlebot3_teleop turtlebot3_teleop_key
任务完成
2258
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
