旅游网站建设模板下载,手游推广平台,企业整站推广,建筑工程分包信息网络平台文章目录 前言一、机器人编队前的准备二、配置仿真环境2.编写机器人编队.cpp文件 三、三角形编队测试 前言
前阵子一直想要实现多机器人编队#xff0c;找到了很多开源的编队代码#xff0c;经过好几天的思索#xff0c;终于实现了在gazebo环境中的TB3三角形机器人编队。 一… 文章目录 前言一、机器人编队前的准备二、配置仿真环境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 turtlesim4新建广播以及接收广播的对应的.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.cpp5创建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的位置信息) gazeboplugin namebase_waffle_controller filenamelibgazebo_ros_p3d.soalwaysOntrue/alwaysOnupdateRate50.0/updateRatebodyNamebase_footprint/bodyNametopicNamebase_pose_ground_truth/topicNamegaussianNoise0.01/gaussianNoiseframeNameworld/frameNamexyzOffsets0 0 0/xyzOffsetsrpyOffsets0 0 0/rpyOffsets/plugin/gazebo3编写机器人gazebo仿真环境 打开turtlebot3_simulations-turtlebot3_gazebo根据自己设计需要设置launch文件这里为方便演示我在multi_turtlebot3.launch文件的基础上进行修改这里我只添加了三个机器人。 代码如下
launcharg namemodel default$(env TURTLEBOT3_MODEL) docmodel type [burger, waffle, waffle_pi]/arg namefirst_tb3 defaulttb3_0/arg namesecond_tb3 defaulttb3_1/arg namethird_tb3 defaulttb3_2/arg namefirst_tb3_x_pos default 1.0/arg namefirst_tb3_y_pos default 0.0/arg namefirst_tb3_z_pos default 0.0/arg namefirst_tb3_yaw default 0.0/arg namesecond_tb3_x_pos default 0.0/arg namesecond_tb3_y_pos default-1.0/arg namesecond_tb3_z_pos default 0.0/arg namesecond_tb3_yaw default 0.0/arg namethird_tb3_x_pos default 0.0/arg namethird_tb3_y_pos default 1.0/arg namethird_tb3_z_pos default 0.0/arg namethird_tb3_yaw default 0.0/include file$(find gazebo_ros)/launch/empty_world.launcharg nameworld_name value$(find turtlebot3_gazebo)/worlds/empty.world/arg namepaused valuefalse/arg nameuse_sim_time valuetrue/arg namegui valuetrue/arg nameheadless valuefalse/arg namedebug valuefalse//include group ns $(arg first_tb3)param namerobot_description command$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro /node pkgrobot_state_publisher typerobot_state_publisher namerobot_state_publisher outputscreenparam namepublish_frequency typedouble value50.0 /param nametf_prefix value$(arg first_tb3) //nodenode namespawn_urdf pkggazebo_ros typespawn_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 //groupgroup ns $(arg second_tb3)param namerobot_description command$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro /node pkgrobot_state_publisher typerobot_state_publisher namerobot_state_publisher outputscreenparam namepublish_frequency typedouble value50.0 /param nametf_prefix value$(arg second_tb3) //nodenode namespawn_urdf pkggazebo_ros typespawn_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 //groupgroup ns $(arg third_tb3)param namerobot_description command$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro /node pkgrobot_state_publisher typerobot_state_publisher namerobot_state_publisher outputscreenparam namepublish_frequency typedouble value50.0 /param nametf_prefix value$(arg third_tb3) //nodenode namespawn_urdf pkggazebo_ros typespawn_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/launch4运行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.cpptb3_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.hint 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.serviceClientturtlesim::Spawn(/spawn);//turtlesim::Spawn srv;//add_turtle.call(srv);// 创建发布tb3_1速度控制指令的发布者ros::Publisher tb3_1_vel node.advertisegeometry_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.hint 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.serviceClientturtlesim::Spawn(/spawn);//turtlesim::Spawn srv;//add_turtle.call(srv);// 创建发布tb3_1速度控制指令的发布者ros::Publisher tb3_1_vel node.advertisegeometry_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.hint 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.serviceClientturtlesim::Spawn(/spawn);//turtlesim::Spawn srv;//add_turtle.call(srv);// 创建发布tb3_1速度控制指令的发布者ros::Publisher tb3_2_vel node.advertisegeometry_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的名称需要和添加的小车机器人名称一一对应。 代码如下 launchnode pkgturtlebot3_teams_zhou typetb3_tf_broadcaster1args/tb3_0 namerobot_0_tf_broadcaster /node pkgturtlebot3_teams_zhou typetb3_tf_broadcaster2args/tb3_1 namerobot_1_tf_broadcaster /node pkgturtlebot3_teams_zhou typetb3_tf_broadcaster3args/tb3_2 namerobot_2_tf_broadcaster /node pkgturtlebot3_teams_zhou typetb3_tf_listener1namefollower1 /node pkgturtlebot3_teams_zhou typetb3_tf_listener2namefollower2 //launch4编译工作环境 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_make2运行机器人仿真环境
roslaunch turtlebot3_gazebo multi_turtlebot3.launch3启动编队程序
roslaunch turtlebot3_teams_zhou turtlebot3_teams_follow_wang.launch 4控制tb3_0小车进行运动
ROS_NAMESPACEtb3_0 rosrun turtlebot3_teleop turtlebot3_teleop_key三角形编队
