ROS----小乌龟之你追我赶

生成八个小乌龟，分别命名为/turtle1、/turtle2、/turtle3、/turtle4、/turtle5、/turtle6、/turtle7、/turtle8然后实现turtle1自己运动，然后turtle2追turtle1，记为turtle2->turtle1。最终实现turtle8->turtle7->turtle6->turtle5->turtle4->turtle3->turtle2->turtle1
这其中涉及到tf变换相关的内容:
在该项目中有三个文件pursue_turtle_control.cpp、pursue_turtle.cpp、pursue_turtle.launch
其中，pursue_turtle.cpp用来发布某一个小乌龟和世界坐标的tf变换，根据传入的参数，确定到底是哪一个小乌龟。
pursue_turtle_control.cpp中产生所需要的小乌龟，同时查询tf树，确定相邻两个乌龟之间的相对方向，并发布对应乌龟的运动topic。
pursue_turtle.launch是launch文件主要用来配置一些参数和同时启动多个节点。

在pursue_turtle.cpp中，首先订阅传入名字小乌龟的/pose，得到对应小乌龟的位置(x，y)和角度theta。然后在对应的消息订阅回调函数中得到的信息进行相应的计算，得到平移和旋转向量，最后打包成固定的格式发布出去。

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>
#include <turtlesim/Pose.h>std::string turtle_name;
void poseCallback(const turtlesim::PoseConstPtr& pose)
{static tf::TransformBroadcaster br;tf::Transform transform;transform.setOrigin(tf::Vector3(pose->x,pose->y,0));tf::Quaternion q;q.setRPY(0,0,pose->theta);transform.setRotation(q);br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"world",turtle_name));
}int main(int argc, char** argv)
{ros::init(argc,argv,"my_life");turtle_name = argv[1];ros::NodeHandle n;ros::Subscriber sub = n.subscribe(turtle_name+"/pose", 10, poseCallback);ros::spin();return 0;
}

在pursue_turtle_control.cpp中，首先是生成所需要的乌龟，乌龟的位置随机产生。

ros::service::waitForService("spawn");// generation turtleros::ServiceClient client = n.serviceClient<turtlesim::Spawn>("spawn");ros::ServiceClient cl = n.serviceClient<std_srvs::Empty>("clear");std_srvs::Empty e;turtlesim::Spawn turtle;srand((unsigned int)time(NULL));for(int i = 2;i < 9;i++){turtle.request.x = rand()%8+1;turtle.request.y = rand()%8+1;client.call(turtle);}

接下来在tf树中查找相应的小乌龟之间的变换关系:

 tf::TransformListener listener;while(n.ok()){cl.call(e);tf::StampedTransform transform2,transform3,transform4,transform5,transform6,transform7,transform8;try{listener.waitForTransform("/turtle2","/turtle1",ros::Time(0),ros::Duration(3.0));listener.lookupTransform("/turtle2","/turtle1",ros::Time(0),transform2);listener.waitForTransform("/turtle3","/turtle2",ros::Time(0),ros::Duration(3.0));listener.lookupTransform("/turtle3","/turtle2",ros::Time(0),transform3);listener.waitForTransform("/turtle4","/turtle3",ros::Time(0),ros::Duration(3.0));listener.lookupTransform("/turtle4","/turtle3",ros::Time(0),transform4);listener.waitForTransform("/turtle5","/turtle4",ros::Time(0),ros::Duration(3.0));listener.lookupTransform("/turtle5","/turtle4",ros::Time(0),transform5);listener.waitForTransform("/turtle6","/turtle5",ros::Time(0),ros::Duration(3.0));listener.lookupTransform("/turtle6","/turtle5",ros::Time(0),transform6);listener.waitForTransform("/turtle7","/turtle6",ros::Time(0),ros::Duration(3.0));listener.lookupTransform("/turtle7","/turtle6",ros::Time(0),transform7);listener.waitForTransform("/turtle8","/turtle1",ros::Time(0),ros::Duration(3.0));listener.lookupTransform("/turtle8","/turtle1",ros::Time(0),transform8);}catch(tf::TransformException& ex){ROS_ERROR("%s",ex.what());ros::Duration(1.0).sleep();continue;}

最后将查询到的变换关系转换成相应的运动:

 geometry_msgs::Twist t;t.angular.z = 1.0*atan2(transform2.getOrigin().y(),transform2.getOrigin().x());;t.linear.x =0.5*sqrt(pow(transform2.getOrigin().x(),2)+pow(transform2.getOrigin().y(),2));;pub2.publish(t);t.angular.z = 1.0*atan2(transform3.getOrigin().y(),transform3.getOrigin().x());;t.linear.x = 0.5*sqrt(pow(transform3.getOrigin().x(),2)+pow(transform3.getOrigin().y(),2));;pub3.publish(t);t.angular.z = 1.0*atan2(transform4.getOrigin().y(),transform4.getOrigin().x());;t.linear.x = 0.5*sqrt(pow(transform4.getOrigin().x(),2)+pow(transform4.getOrigin().y(),2));;pub4.publish(t);t.angular.z = 1.0*atan2(transform5.getOrigin().y(),transform5.getOrigin().x());;t.linear.x = 0.5*sqrt(pow(transform5.getOrigin().x(),2)+pow(transform5.getOrigin().y(),2));;pub5.publish(t);t.angular.z = 1.0*atan2(transform6.getOrigin().y(),transform6.getOrigin().x());;t.linear.x = 0.5*sqrt(pow(transform6.getOrigin().x(),2)+pow(transform6.getOrigin().y(),2));;pub6.publish(t);t.angular.z = 1.0*atan2(transform7.getOrigin().y(),transform7.getOrigin().x());;t.linear.x = 0.5*sqrt(pow(transform7.getOrigin().x(),2)+pow(transform7.getOrigin().y(),2));;pub7.publish(t);t.angular.z = 1.0*atan2(transform8.getOrigin().y(),transform8.getOrigin().x());;t.linear.x = 0.5*sqrt(pow(transform8.getOrigin().x(),2)+pow(transform8.getOrigin().y(),2));;pub8.publish(t);

完整的代码:

#include <ros/ros.h>
#include <turtlesim/Spawn.h>
#include <geometry_msgs/Twist.h>
#include <tf/transform_listener.h>#include <std_srvs/Empty.h>#include <ctime>int main(int argc,char** argv)
{ros::init(argc,argv,"control");ros::NodeHandle n;ros::service::waitForService("spawn");// generation turtleros::ServiceClient client = n.serviceClient<turtlesim::Spawn>("spawn");ros::ServiceClient cl = n.serviceClient<std_srvs::Empty>("clear");std_srvs::Empty e;turtlesim::Spawn turtle;srand((unsigned int)time(NULL));for(int i = 2;i < 9;i++){turtle.request.x = rand()%8+1;turtle.request.y = rand()%8+1;client.call(turtle);}ros::Publisher pub1 = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);ros::Publisher pub2 = n.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);ros::Publisher pub3 = n.advertise<geometry_msgs::Twist>("/turtle3/cmd_vel", 10);ros::Publisher pub4 = n.advertise<geometry_msgs::Twist>("/turtle4/cmd_vel", 10);ros::Publisher pub5 = n.advertise<geometry_msgs::Twist>("/turtle5/cmd_vel", 10);ros::Publisher pub6 = n.advertise<geometry_msgs::Twist>("/turtle6/cmd_vel", 10);ros::Publisher pub7 = n.advertise<geometry_msgs::Twist>("/turtle7/cmd_vel", 10);ros::Publisher pub8 = n.advertise<geometry_msgs::Twist>("/turtle8/cmd_vel", 10);tf::TransformListener listener;while(n.ok()){cl.call(e);tf::StampedTransform transform2,transform3,transform4,transform5,transform6,transform7,transform8;try{listener.waitForTransform("/turtle2","/turtle1",ros::Time(0),ros::Duration(3.0));listener.lookupTransform("/turtle2","/turtle1",ros::Time(0),transform2);listener.waitForTransform("/turtle3","/turtle2",ros::Time(0),ros::Duration(3.0));listener.lookupTransform("/turtle3","/turtle2",ros::Time(0),transform3);listener.waitForTransform("/turtle4","/turtle3",ros::Time(0),ros::Duration(3.0));listener.lookupTransform("/turtle4","/turtle3",ros::Time(0),transform4);listener.waitForTransform("/turtle5","/turtle4",ros::Time(0),ros::Duration(3.0));listener.lookupTransform("/turtle5","/turtle4",ros::Time(0),transform5);listener.waitForTransform("/turtle6","/turtle5",ros::Time(0),ros::Duration(3.0));listener.lookupTransform("/turtle6","/turtle5",ros::Time(0),transform6);listener.waitForTransform("/turtle7","/turtle6",ros::Time(0),ros::Duration(3.0));listener.lookupTransform("/turtle7","/turtle6",ros::Time(0),transform7);listener.waitForTransform("/turtle8","/turtle1",ros::Time(0),ros::Duration(3.0));listener.lookupTransform("/turtle8","/turtle1",ros::Time(0),transform8);}catch(tf::TransformException& ex){ROS_ERROR("%s",ex.what());ros::Duration(1.0).sleep();continue;}geometry_msgs::Twist t;t.angular.z = 1.0*atan2(transform2.getOrigin().y(),transform2.getOrigin().x());;t.linear.x =0.5*sqrt(pow(transform2.getOrigin().x(),2)+pow(transform2.getOrigin().y(),2));;pub2.publish(t);t.angular.z = 1.0*atan2(transform3.getOrigin().y(),transform3.getOrigin().x());;t.linear.x = 0.5*sqrt(pow(transform3.getOrigin().x(),2)+pow(transform3.getOrigin().y(),2));;pub3.publish(t);t.angular.z = 1.0*atan2(transform4.getOrigin().y(),transform4.getOrigin().x());;t.linear.x = 0.5*sqrt(pow(transform4.getOrigin().x(),2)+pow(transform4.getOrigin().y(),2));;pub4.publish(t);t.angular.z = 1.0*atan2(transform5.getOrigin().y(),transform5.getOrigin().x());;t.linear.x = 0.5*sqrt(pow(transform5.getOrigin().x(),2)+pow(transform5.getOrigin().y(),2));;pub5.publish(t);t.angular.z = 1.0*atan2(transform6.getOrigin().y(),transform6.getOrigin().x());;t.linear.x = 0.5*sqrt(pow(transform6.getOrigin().x(),2)+pow(transform6.getOrigin().y(),2));;pub6.publish(t);t.angular.z = 1.0*atan2(transform7.getOrigin().y(),transform7.getOrigin().x());;t.linear.x = 0.5*sqrt(pow(transform7.getOrigin().x(),2)+pow(transform7.getOrigin().y(),2));;pub7.publish(t);t.angular.z = 1.0*atan2(transform8.getOrigin().y(),transform8.getOrigin().x());;t.linear.x = 0.5*sqrt(pow(transform8.getOrigin().x(),2)+pow(transform8.getOrigin().y(),2));;pub8.publish(t);t.angular.z = rand()%3;t.linear.x = rand()%7;pub1.publish(t);ros::spinOnce();}return 0;
}

最后看一下pursue_turtle.launch文件:

<launch>//用来启动显示小乌龟的界面<node pkg="turtlesim" type="turtlesim_node" name="star"/>//启动pursue_turtle_control节点，产生小乌龟<node pkg="base" type="pursue_turtle_control" name="road"/>//启动八个节点，发布每个小乌龟和世界坐标之间的变换关系。<node pkg="base" type="pursue_turtle" args="/turtle1" name="turtle1_world_broadcaster"/><node pkg="base" type="pursue_turtle" args="/turtle2" name="turtle2_world_broadcaster"/><node pkg="base" type="pursue_turtle" args="turtle3" name="turtle3_world_broadcaster"/><node pkg="base" type="pursue_turtle" args="turtle4" name="turtle4_world_broadcaster"/><node pkg="base" type="pursue_turtle" args="turtle5" name="turtle5_world_broadcaster"/><node pkg="base" type="pursue_turtle" args="turtle6" name="turtle6_world_broadcaster"/><node pkg="base" type="pursue_turtle" args="turtle7" name="turtle7_world_broadcaster"/><node pkg="base" type="pursue_turtle" args="turtle8" name="turtle8_world_broadcaster"/></launch>

ROS----小乌龟之你追我赶相关推荐

ROS小乌龟走设定图形路线（键盘控制+Python代码实现）
相信在很多人学习ROS的时候都会写一下这个demo,不仅是对代码能力的考察(代码语法.结构都还是相对简单的),还是对ROS话题通信这些基础概念的理解的考量. 首先命令运行 roscore rosrun ...
ROS：编写节点，让ROS小乌龟画圆和矩形
操作系统:ubuntu1404,ROS indigo 软件:Roboware 实验开始在my_turtle_package(自己创建的package)->src 中创建cpp文件:(如下图) ...
ROS小乌龟turtlesim详解
turtlesim 小乌龟模拟小乌龟的启动在安装完ROS之后,就可以启动小乌龟了,打开一个终端. 1. 首先要打开ROS服务 roscore 2. 打开一个新终端,打开小乌龟 rosrun tur ...
ROS语音控制——小乌龟按设定图形路线运动
最近几天在学习语音识别.语音合成,应用到ROS当中实现一些简单的案例,下面是使用语音来控制turtle小乌龟走设定图形路线的案例. ROS小乌龟走设定图形路线(键盘控制+Python代码实现)_笨小古 ...
ROS知识：分析和改写小乌龟代码【01】
ROS之游戏手柄控制乌龟和机器人_u014587147的专栏-CSDN博客本篇对小乌龟的项目进行分析,看看一个普通的ROS项目到底如何构建,才能算完整合法工程:学完后,期望可以开发我们自己的项目. ...
ROS安装并运行小乌龟
镜像选择: 2. ROS kinetic安装与使用爲了保證安裝速度,請選擇國內鏡像如下:ustc鏡像 2.1 设置安装源 ROS Kinetic只支持Wily(15.10)和Xenial(16.04 ...
ROS入门小乌龟跟随示例
一.不使用TF转换的方法 #include <ros/ros.h> #include <geometry_msgs/Twist.h> #include<math.h> ...
ROS修改小乌龟程序背景颜色
我们在之前的编程中可以发现,小乌龟程序的背景颜色是蓝色,可能大家看的比较眼乏了,那我们接下来可以编程修改小乌龟程序的背景颜色. 首先,我们输入rosparam list,可以看到三个关于小乌龟程序背景 ...
ROS学习笔记之小乌龟跟随
ROS学习笔记之小乌龟跟随说明:整个案例是跟着赵虚左老师的视频和文档资料学习的,特此感谢赵虚左老师和Autolabor官方文档地址视频地址学习案例之前的预备知识:TF坐标变换大体实现流程: ...
ros入门保姆级教程之召唤小乌龟
(我所使用的版本是Ubuntu20.04,安装ros请参考链接https://blog.csdn.net/qq_46106285/article/details/120982412?spm=1001. ...

ROS----小乌龟之你追我赶

ROS----小乌龟之你追我赶

ROS----小乌龟之你追我赶相关推荐

最新文章

热门文章