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

最新推荐文章于 2023-05-25 19:28:05 发布
最新推荐文章于 2023-05-25 19:28:05 发布
阅读量1.5k 收藏 27
点赞数 4
分类专栏： ROS 文章标签： ros turtle tf
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/qq_25105061/article/details/103896397
版权

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 turtle
    ros::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 turtle
    ros::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>
Dynamicw
关注
  • 4
    点赞
  • 踩
  • 27
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
ROS2 入门应用 创建启动文件（Python）
氢键H-H
06-18 1万+
ROS2 入门应用 创建启动文件（Python）
ROS控制小乌龟程序
12-11
文章：ROS：编写节点，让ROS小乌龟画圆和矩形 作者：ID：IMBA_09
ROS 乌龟追逐实验
Sunxj的博客
07-17 1826
开始准备在gazebo中模拟小车跟随的实验，苦于没有思路，网上很多直接抛程序，让我连编写结构都没想法，想到了ros自带的小海龟实验，想从这里入手，一步步学吧。 下面是跟随实验的安装过程和运行方式： 1.安装实验包 sudo apt-get install ros-melodic-turtle-tf 我使用ros版本是melodic，如果是其他版本更换一下 2.打开界面，运行小海龟 roslaunch turtle_tf turtle_tf_demo.launch 3.打开键盘控制..
ROS学习——利用tf坐标变换实现乌龟B追乌龟A
bulletstart的博客
05-25 339
会出现小乌龟，将鼠标放在终端2中，就可以通过键盘上下左右键控制小乌龟A的运动，小乌龟B就会跟上来。
ROS中的tf(transform)的理解 ，你追我小乌龟的深入剖析
热门推荐
凌少skier的小窝
08-16 1万+
对于ros中的tf其实一直理解不是很深，最近工作上一直在用，就很懵逼，出来混果然是要还的～于是这两天把ros官方提供的小乌龟版的你追我，如果你追到我，我就让你xxx，一个一个命令跑了一下，现在有了一个较深的理解，如果哪里说的不对，还请指正。 首先看launch文件内容 <launch> <!-- 海龟仿真器 --> <node pkg="turtle...
ROS坐标变换——海龟的跟随移动
mantou_riji的博客
03-07 1033
1.下载 tf 功能包 sudo apt-get install ros-melodic-turtle-tf 2.roslaunch来启动脚本文件 roslaunch turtle_tf turtle_tf_demo.launch $ rosrun turtlesim turtle_teleop_key $ rosrun tf view_frames
ROS-CAN通信协议程序
12-13
"ROS-CAN通信协议程序"是一个将ROS与CAN通信相结合的应用，它允许ROS节点通过CAN总线与其他设备或系统进行交互。在这个程序中，`CANalyst-II`是一款用于CAN数据分析的工具，它提供了Linux版本，可以方便地在Linux...
ros-yolov5-ws
07-14
在本项目"ros-yolov5-ws"中，我们将看到如何在ROS Melodic版本中集成YOLOv5，实现目标检测并发布处理后的数据。 首先，了解YOLOv5。YOLOv5是一种基于深度学习的一阶段目标检测框架，它能够直接从图像中预测边界框和...
Ros-melodic安装包
12-04
如果你想只安装ROS Melodic的基本工具和库，可以使用`ros-melodic-ros-base`代替`ros-melodic-desktop-full`。 **六、环境配置** 每次启动新终端时，你需要初始化ROS环境。将以下行添加到你的`~/.bashrc`文件末尾：...
适用于ROS-noetic的robotiq包
最新发布
05-09
ROS-noetic是ROS的一个发行版本，它提供了一套完整的工具、库和协议，用于构建、编写和部署机器人软件。Robotiq是一家专注于机器人夹爪和传感器的公司，其产品广泛应用于工业自动化领域。 在ROS-noetic中使用的...
ros-melodic-desktop-full 源码
12-15
在"ros-melodic-desktop-full"源码中，你可以找到构建整个ROS Melodic Desktop Full所需的所有源代码。这涵盖了ROS的核心库、开发者工具、以及各种机器人相关的软件包。"ros_catkin_ws"是ROS中常见的工作空间...
ros多个小乌龟_ROS中常用组件
weixin_42127754的博客
01-02 443
写在前面：前面两篇写作风格不太统一，有的是摘抄原书，有的加入自己理解。这里说明下，本学习笔记主要是读书笔记，因为我看过好几遍了，且用了挺久的ROS，所以如果没有读者一定基础，可能会无法明白笔记中的一些限定和共识内容。如果有疑问，欢迎评论留言交流。大家有觉得需要特别强调或者自己对某一部分有更好的理解方式，也欢迎评论补充。接下来我打算：我比较熟悉的部分，可能只写一些关键知识点；不熟悉（平常用的少的）可...
实例 3——Python 实现小乌龟的旋转运动(ROS)
m0_68094390的博客
05-19 989
该实例包含两个功能，一个是小乌龟旋转指定角度（rotate），另一个则是小乌龟旋转到指定角。第 29~35 行：因为小乌龟只需要执行旋转操作，因此需要把角速度和线速度都设置为 0。第 41~44 行：如果角速度大于 0，则逆时针旋转，否则就顺时针旋转。第 61~63 行：当前旋转的角度大于要旋转的角度，那么就表明到达。第 57 行：计算从起始时间开始到当前时间，小乌龟转过的角度。执行如6.12所示的指令就可以实现小乌龟的旋转。第 72 行：把要旋转到的目标角度转化为弧度。
ROS——话题及使用C++代码控制小海龟走一个圆形
qq_52187415的博客
03-04 1787
这次实验是继用键盘控制小海龟移动后的进一步实验步骤,熟悉了ROS的消息和话题的收发机制.直观上也了解了小海龟的运动机制最后我们实现了编程方式控制小海龟圆周运动,对ROS也有了更进一步的了解。
【ROS通信机制实战练习三】通过服务实现turtlesim小乌龟的生成
丑小鸭
12-26 525
【ROS通信机制实战练习三】通过服务实现turtlesim小乌龟的生成
ROS服务通信应用（1）：创建服务客户端，生成多只乌龟
weixin_52029211的博客
01-28 3447
需求： 在turtlesim仿真环境中，写一个C++程序，创建客户端，请求/spawn服务，生成多只乌龟 实现效果： 服务通信常用命令行工具: rosservice list 服务列表 rosservice type /spawn 消息类型 rossrv info turtlesim/Spawn 消息格式 rosservice call /clear 清除功能(可以清理小乌龟走过的线路) rosservice call /kill “name: ‘turtle1’” 杀死小乌龟 ross
ROS坐标变换，B乌龟跟随A乌龟运动（python）
weixin_72050316的博客
01-08 955
ROS坐标变换，B乌龟跟随A乌龟运动（python）
ROS动作编程（小海龟运动）
qq_53088119的博客
03-15 1522
ROS动作编程小海龟指定位置移动
【ROS实操3服务调用添加乌龟数量】
weixin_47869094的博客
04-16 979
分别是x，y坐标，唯一需要注意的是theta是乌龟的脑袋朝向，是用弧度来做单位的，3.14为一弧度，要想朝上，就是一半的弧度，就是1.57.编码实现向turtlesim 发送请求，在乌龟显示节点的窗体指定位置生成一乌龟，这是一个服务请求操作。用c++实现，需要的功能包有roscpp，rospy，std_msgs,turtlesim。3.启动roscore、turtlesim_node、乌龟生成节点，生成新的乌龟。2.要通过ROS命令，来获取乌龟生成服务的服务名称以及服务消息类型。2.编码实现服务请求节点。
ros---小乌龟画爱心
06-10
然后，您需要使用Turtlebot3的控制命令来控制小乌龟机器人的运动。最后，您可以使用Python等编程语言编写程序来控制小乌龟机器人在屏幕上画出爱心形状。具体实现方法可以参考ROS官方文档和Turtlebot3的相关教程。...
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值