【ROS程序】 --- 2-3. TF--实现乌龟跟随

最新推荐文章于 2024-04-25 10:45:14 发布
最新推荐文章于 2024-04-25 10:45:14 发布
阅读量1.8k 收藏 10
点赞数
分类专栏： # ROS进阶程序 文章标签： ubuntu c++ linux 自动驾驶 嵌入式
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/Eric_Sober/article/details/122213137
版权

“那就自己沉淀下来！”

本节综合TF知识，实现一只乌龟跟随另一只乌龟的任务。

I、准备工作

延用工作空间test2_ws;
功能包demo2_trans_pose存放代码；
新建乌龟的代码放在03creat_turtle.cpp中；
广播乌龟位置信息代码放在03pub_pose.cpp中。
接受广播，完成坐标转换，发布第二只乌龟的信息放在03sub_pose.cpp中。

我们的思路：乌龟中有三个坐标系，world,turtle1,turtle2。1创建turtle2用来跟随，2将turtle1和turtle2的坐标信息（实际上就是turtle1和turtle2之间的坐标系信息）传到广播器，3订阅到广播器内容后，完成坐标转换。

II、创建乌龟程序的思路

  • 第一种方法是在ROS基础知识中学到的，通过rosservice调用/spawn来生成
rosservice call /spawn "x: 1.0
y: 1.0
theta: 1.0
name: 'turtle_flow'"

-第二种是用程序生成，然后编写客户端调用服务

	ros::service::waitForService("/spawn");
    turtlesim::Spawn spawn;
    spawn.request.name = "turtle2";
    spawn.request.x = 1.0;
    spawn.request.y = 2.0;
    spawn.request.theta = 3.12415926;
    bool flag = client.call(spawn);

III、广播乌龟位姿程序的思路

  1. 首先订阅两只乌龟的位置信息话题，得到它们坐标：
ros::Subscriber sub = nh.subscribe<turtlesim::Pose>(turtle_name + "/pose",1000,doPose);
  1. 分别进入回调函数完成常规的广播操作
    这里注意此时发送时坐标系都是world坐标系，对应的子坐标系才是自己名字。
	static tf2_ros::TransformBroadcaster broadcaster;
    //  6-2.将 pose 信息转换成 TransFormStamped
    geometry_msgs::TransformStamped tfs;
    tfs.header.frame_id = "world";

3.填充数据然后发送

 broadcaster.sendTransform(tfs);

IV、接受乌龟广播，坐标变换

  1. 创建TF订阅对象
	tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);
  1. 创建控制第二只乌龟的发布器
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",1000);
  1. 获取两个坐标系的相对信息(注意顺序)
    这里的ros::Time(0)，表示从最近的信息开始提取，而不采用now,如果用now会有很大延迟。这里是将乌龟2的坐标映射到turtle1坐标系上。这样turtle1永远是（0，0）。(子坐标系，父坐标系)
geometry_msgs::TransformStamped tfs = buffer.lookupTransform("turtle2","turtle1",ros::Time(0));
  1. 填充乌龟2信息实现发布：
 			geometry_msgs::Twist twist;
            twist.linear.x =  0.5*sqrt(pow(tfs.transform.translation.x,2) + pow(tfs.transform.translation.y,2));
            twist.angular.z = 5 * atan2(tfs.transform.translation.y,   tfs.transform.translation.x);
			pub.publish(twist);

turtle2在turtle1坐标系下，方向取正切加系数。

V、launch文件编写

实现流程：

  1. 运行turtlesim_node
  2. 运行turtle_deleop
  3. 运行创建乌龟2节点
  4. 运行我们的发布节点，发布乌龟1的信息，再发布乌龟2的信息
  5. 运行我们的收听节点，控制乌龟2

简单起见，我们封装在一个launch文件中：

<launch>
    <!-- 启动乌龟节点与键盘控制节点 -->
    <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
    <node pkg="turtlesim" type="turtle_teleop_key" name="key_control" launch-prefix="xterm -e"/>
    <!-- 启动创建第二只乌龟的节点 -->
    <node pkg="demo2_trans_pose" type="creat_turtle2" name="turtle2" output="screen" />
    <!-- 启动两个坐标发布节点 -->
    <node pkg="demo2_trans_pose" type="pub_tf_turtle" name="pub_turtle1" args="turtle1" output="screen" />
    <node pkg="demo2_trans_pose" type="pub_tf_turtle" name="pub_turtle2" args="turtle2" output="screen" />
    <!-- 启动坐标转换节点 -->
    <node pkg="demo2_trans_pose" type="sub_tf_turtle" name="sub_follow" output="screen" />
</launch>

然后打开rviz ，添加tf坐标，全局坐标选择world，控制乌龟运动，结果如下：

在这里插入图片描述
打开rviz，如下
在这里插入图片描述

VI 全部源码

03creat_turtle.cpp

/* 
    创建第二只小乌龟
 */
#include "ros/ros.h"
#include "turtlesim/Spawn.h"

int main(int argc, char *argv[])
{

    setlocale(LC_ALL,"");

    //执行初始化
    ros::init(argc,argv,"create_turtle2");
    //创建节点
    ros::NodeHandle nh;
    //创建服务客户端
    ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");

    ros::service::waitForService("/spawn");
    turtlesim::Spawn spawn;
    spawn.request.name = "turtle2";
    spawn.request.x = 1.0;
    spawn.request.y = 2.0;
    spawn.request.theta = 3.12415926;
    bool flag = client.call(spawn);
    if (flag)
    {
        ROS_INFO("乌龟%s创建成功!",spawn.response.name.c_str());
    }
    else
    {
        ROS_INFO("乌龟2创建失败!");
    }

    ros::spin();

    return 0;
}

03pub_pose.cpp

/*  
    该文件实现:需要订阅 turtle1 和 turtle2 的 pose，然后广播相对 world 的坐标系信息

    注意: 订阅的两只 turtle,除了命名空间(turtle1 和 turtle2)不同外,
          其他的话题名称和实现逻辑都是一样的，
          所以我们可以将所需的命名空间通过 args 动态传入

    实现流程:
        1.包含头文件
        2.初始化 ros 节点
        3.解析传入的命名空间
        4.创建 ros 句柄
        5.创建订阅对象
        6.回调函数处理订阅的 pose 信息
            6-1.创建 TF 广播器
            6-2.将 pose 信息转换成 TransFormStamped
            6-3.发布
        7.spin

*/
//1.包含头文件
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2/LinearMath/Quaternion.h"
#include "geometry_msgs/TransformStamped.h"
//保存乌龟名称
std::string turtle_name;


void doPose(const turtlesim::Pose::ConstPtr& pose)
{
    //  6-1.创建 TF 广播器 ---------------------------------------- 注意 static
    static tf2_ros::TransformBroadcaster broadcaster;
    //  6-2.将 pose 信息转换成 TransFormStamped
    geometry_msgs::TransformStamped tfs;
    tfs.header.frame_id = "world";
    tfs.header.stamp = ros::Time::now();
    tfs.child_frame_id = turtle_name;
    tfs.transform.translation.x = pose->x;
    tfs.transform.translation.y = pose->y;
    tfs.transform.translation.z = 0.0;
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,pose->theta);
    tfs.transform.rotation.x = qtn.getX();
    tfs.transform.rotation.y = qtn.getY();
    tfs.transform.rotation.z = qtn.getZ();
    tfs.transform.rotation.w = qtn.getW();
    //  6-3.发布
    broadcaster.sendTransform(tfs);

} 

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ros 节点
    ros::init(argc,argv,"pub_turtle_tf");
    // 3.解析传入的命名空间
    
    if (argc != 2)
    {
        ROS_ERROR("请传入正确的参数");
    } 
    else
     {
        turtle_name = argv[1];
        ROS_INFO("乌龟 %s 坐标发送启动",turtle_name.c_str());
    }

    // 4.创建 ros 句柄
    ros::NodeHandle nh;
    // 5.创建订阅对象
    ros::Subscriber sub = nh.subscribe<turtlesim::Pose>(turtle_name + "/pose",1000,doPose);
    //     6.回调函数处理订阅的 pose 信息
    //         6-1.创建 TF 广播器
    //         6-2.将 pose 信息转换成 TransFormStamped
    //         6-3.发布
    // 7.spin
    ros::spin();
    return 0;
}

03sub_pose.cpp

/*  
    订阅 turtle1 和 turtle2 的 TF 广播信息，查找并转换时间最近的 TF 信息
    将 turtle1 转换成相对 turtle2 的坐标，在计算线速度和角速度并发布

    实现流程:
        1.包含头文件
        2.初始化 ros 节点
        3.创建 ros 句柄
        4.创建 TF 订阅对象
        5.处理订阅到的 TF
        6.spin

*/
//1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/Twist.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ros 节点
    ros::init(argc,argv,"sub_turtle_tf");
    // 3.创建 ros 句柄
    ros::NodeHandle nh;
    // 4.创建 TF 订阅对象
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);
    // 5.处理订阅到的 TF

    // 需要创建发布 /turtle2/cmd_vel 的 publisher 对象

    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",1000);

    ros::Rate rate(10);
    while (ros::ok())
    {
        try
        {

            //5-1.先获取 turtle1 相对 turtle2 的坐标信息
            geometry_msgs::TransformStamped tfs = buffer.lookupTransform("turtle2","turtle1",ros::Time(0));
            //5-2.根据坐标信息生成速度信息 -- geometry_msgs/Twist.h
            geometry_msgs::Twist twist;
            twist.linear.x =  0.5*sqrt(pow(tfs.transform.translation.x,2) + pow(tfs.transform.translation.y,2));
            twist.angular.z = 5 * atan2(tfs.transform.translation.y,   tfs.transform.translation.x);

            //5-3.发布速度信息 -- 需要提前创建 publish 对象
            pub.publish(twist);
        }
        catch(const std::exception& e)
        {
            // std::cerr << e.what() << '\n';
            ROS_INFO("错误提示:%s",e.what());
        }



        rate.sleep();
        // 6.spin
        ros::spinOnce();
    }

    return 0;
}
终问鼎
关注
  • 0
    点赞
  • 踩
  • 10
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
【ROS实操3服务调用添加乌龟数量】
weixin_47869094的博客
04-16 967
分别是x，y坐标，唯一需要注意的是theta是乌龟的脑袋朝向，是用弧度来做单位的，3.14为一弧度，要想朝上，就是一半的弧度，就是1.57.编码实现向turtlesim 发送请求，在乌龟显示节点的窗体指定位置生成一乌龟，这是一个服务请求操作。用c++实现，需要的功能包有roscpp，rospy，std_msgs,turtlesim。3.启动roscore、turtlesim_node、乌龟生成节点，生成新的乌龟。2.要通过ROS命令，来获取乌龟生成服务的服务名称以及服务消息类型。2.编码实现服务请求节点。
基于C++实现的基于ROS的多无人机编队仿真
最新发布
06-07
【作品名称】：基于C++实现的基于ROS的多无人机编队仿真 【适用人群】：适用于希望学习不同技术领域的小白或进阶学习者。可作为毕设项目、课程设计、大作业、工程实训或初期项目立项。 【项目介绍】：基于C++实现的基于ROS的多无人机编队仿真
ROS学习（第二篇）---小海龟仿真和工作空间创建以及配置CMakeLists.txt
lifeisme666的博客
02-28 502
ROS学习（第二篇）—小海龟仿真 首先打开终端ctrl+alt+T，然后输入第一个命令 rescore 然后再次重新打开另一个终端ctrl+alt+T，然后输入第二个命令 rosrun turtlesim turtlesim_node 然后再次重新打开另一个终端ctrl+alt+T，然后输入第三个命令 rosrun turtlesim turtle_teleop_key 然后按方向键控制海龟运动 ...
ROS python实现乌龟跟随
El_ia_uk的博客
04-25 590
产生两只乌龟，中间的乌龟(A) 和 左下乌龟(B), B 会自动运行至A的位置，并且键盘控制时，只是控制 A 的运动，但是 B 可以跟随 A 运行乌龟跟随实现的核心，是乌龟A和B都要发布相对世界坐标系的坐标信息，然后，订阅到该信息需要转换获取A相对于B坐标系的信息，最后，再生成速度信息，并控制B运动。Python实现。
ROS TF坐标变化，小乌龟跟随案例
ck784101777的博客
06-02 6550
ROS TF的基础，四元数、欧拉角理论见该篇文章，讲解非常详细 https://blog.csdn.net/zhanghm1995/article/details/84644984 1.程序说明 本实例创建两个乌龟，乌龟1乌龟2，控制乌龟1随意移动，保证乌龟2实时跟随乌龟1。想要达到这个效果，程序设计思路为： 1）我们需要获取乌龟1的位姿信息，本程序只考虑x,y，以及欧拉角的偏航角（Yaw），将乌龟1的位姿变换到世界坐标系（tf变换），并发布乌龟1的位姿信息 2）有了乌龟1的位姿信息，我们还需要.
ros_乌龟跟随
2301_79706435的博客
03-26 387
这里可以发现：这个tf2相关的包可以实现转换坐标的功能，而上一个代码中广播也可以（不过手动），这两个对比起来，tf2更为专业，可以使用少量代码实现相同功能。这里使用的是topic通信，主要实现原理是：turtle运动时会有话题发布位置信息，我们要获得，并转化为相对位置。这里生成小乌龟使用的是服务通信， 可以对该服务进行查看（依次输入下列命令可以查看 )(非必须步骤)这里我还对service进行分析（非必须步骤）四.完整launch。
Ros入门学习003——小乌龟跟随
l434503227的博客
10-01 705
小乌龟跟随实验
ROS学习笔记之小乌龟跟随
Bin_0902的博客
08-14 2676
欢迎理性讨论，共同进步
ROS 运行小乌龟
luoganttcc的博客
06-07 2191
命令的格式为：rosrun [包名] [节点名]。turtlesim是功能包，turtlesim_node是功能包中的一个节点。单独打开一个终端，可以用命令ctrl+shift+t在同一个窗口中打开新的终端。注意，光标必须放在此终端上乌龟才能动.而这个时候打开新的终端运行另一个节点。通过键盘的四个箭头控制乌龟的运动。
ROS-小乌龟跟随
yiluo9918的博客
07-01 1566
TF2 package： tf2是tf的替代版，tf2是一个允许用户随时跟踪多个坐标帧的包。tf在时间缓冲的树结构中维护坐标帧之间的关系，并允许用户在任意两个坐标帧之间任意时间点转换点、向量等。典型应用就是：一只乌龟跟随着被控制的乌龟移动。 官方demo: 1. Set Up the Demo $ sudo apt-get install ros-kinetic-ros-tutorials ros...
ROS入门(十)——两只小乌龟（乌龟跟随C++实现）
热门推荐
Netceor的博客
07-25 1万+
【奥特学园】ROS机器人入门课程《ROS理论与实践》零基础教程P315- 【以上视频笔记见http://www.autolabor.com.cn/book/ROSTutorials/】 前文参考： ROS入门(九)——机器人导航一（介绍、地图、定位和路径规划） 二、地图相关信息 （1）nav_msgs/MapMetaData 地图元数据，包括地图的宽度、高度、分辨率等。调用rosmsg info nav_msgs/MapMetaData显示消息内容 time map_load_time
ros2-web-bridge:将浏览器桥接到ROS 2.0
04-27
ros2-web-bridgerosbridge v2协议的服务器实现ros2-web-bridge利用客户端，通过采用为提供JSON接口。 该网桥可以通过WebSockets上的JSON处理命令。ROS 2支持该ros2幅桥默认支持（目前最新的ROS 2的稳定版本 ），请...
ROS netinstall-6.48.1刷机工具.rar
03-24
ROS，全称Robot Operating System（机器人操作系统），是一个开源操作系统，专为机器人设备和软件开发而设计。它提供了软件框架、工具和库，使得开发者能够轻松地构建复杂的机器人应用。ROS netinstall是ROS系统安装...
ROS-CAN通信协议程序
12-13
"ROS-CAN通信协议程序"是一个将ROS与CAN通信相结合的应用，它允许ROS节点通过CAN总线与其他设备或系统进行交互。在这个程序中，`CANalyst-II`是一款用于CAN数据分析的工具，它提供了Linux版本，可以方便地在Linux...
蓝海激光 LDS-50C-2(3).zip
09-01
最后，"ROS资料.txt"可能包含有关如何将LDS-50C-2(3)与Robot Operating System（ROS）对接的信息。ROS是机器人开发中广泛使用的开源框架，它提供了一套标准化的接口和工具，使得不同硬件设备的集成变得简单。通过ROS...
ros-yolov5-ws
07-14
在本项目"ros-yolov5-ws"中，我们将看到如何在ROS Melodic版本中集成YOLOv5，实现目标检测并发布处理后的数据。 首先，了解YOLOv5。YOLOv5是一种基于深度学习的一阶段目标检测框架，它能够直接从图像中预测边界框和...
ROS TF 变换实现小乌龟跟随
m0_46259216的博客
02-21 3137
实现分析： 核心是生成两只乌龟（采用服务的形式在窗体中生成一只新的乌龟），乌龟A和乌龟B都要发布相对于世界坐标系的坐标信息，然后订阅到该消息转换获取A相对于B坐标系的信息，最后生成速度信息，发布并控制B运动。 创建功能包 相关依赖：tf2、tf2_ros、tf2_geometry_msgs、roscpp、rospy、std_msgs、geometry_msgs、turtlesim。（tf2、tf2_ros、tf2_geometry_msgs是与tf2坐标变换有关的依赖） mkdir -p turt
ROS学习记录（二）玩转海龟——海龟保姆级教程
weixin_54729896的博客
12-30 4135
环境变量的设置，海龟的系列实验（画圆、跟随）
ROS学习3 - launch文件运行小乌龟案例
Baby_of_breath的博客
07-25 1952
launch小乌龟案例，需要依次启动3个终端，实现键盘控制小乌龟的运动。launch文件可以一次性启动3个节点。
Ros小乌龟直线运动
09-05
Ros小乌龟可以通过使用turtle模块来进行直线运动。你可以使用以下代码来让它向前移动一定距离： ```python import turtle # 创建一个画布和一个乌龟对象 canvas = turtle.Screen() ros = turtle.Turtle() # 向前移动100个像素 ros.forward(100) # 关闭画布 canvas.bye() ``` 在这个例子中，我们首先导入了turtle模块，然后创建了一个画布和一个名为ros的乌龟对象。我们使用`ros.forward(100)`命令来让乌龟向前移动100个像素。最后，我们使用`canvas.bye()`命令关闭了画布。 你可以根据需要修改移动的距离、方向和其他属性。希望这能帮到你！如果你还有其他问题，请随时提问。
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明：

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

余额充值