ROS学习第二十三节——TF坐标变换实操

news2024/11/14 13:28:37

1.综述

需求描述:

程序启动之初: 产生两只乌龟,中间的乌龟(A) 和 左下乌龟(B), B 会自动运行至A的位置,并且键盘控制时,只是控制 A 的运动,但是 B 可以跟随 A 运行

结果演示:

实现分析:

乌龟跟随实现的核心,是乌龟A和B都要发布相对世界坐标系的坐标信息,然后,订阅到该信息需要转换获取A相对于B坐标系的信息,最后,再生成速度信息,并控制B运动。

  1. 启动乌龟显示节点
  2. 在乌龟显示窗体中生成一只新的乌龟(需要使用服务)
  3. 编写两只乌龟发布坐标信息的节点
  4. 编写订阅节点订阅坐标信息并生成新的相对关系生成速度信息

实现流程:C++ 与 Python 实现流程一致

  1. 新建功能包,添加依赖

  2. 编写服务客户端,用于生成一只新的乌龟

  3. 编写发布方,发布两只乌龟的坐标信息

  4. 编写订阅方,订阅两只乌龟信息,生成速度信息并发布

  5. 运行

准备工作:

1.了解如何创建第二只乌龟,且不受键盘控制

创建第二只乌龟需要使用rosservice,话题使用的是 spawn

rosservice call /spawn "x: 1.0
y: 1.0
theta: 1.0
name: 'turtle_flow'" 
name: "turtle_flow"

键盘是无法控制第二只乌龟运动的,因为使用的话题: /第二只乌龟名称/cmd_vel,对应的要控制乌龟运动必须发布对应的话题消息

2.了解如何获取两只乌龟的坐标

是通过话题 /乌龟名称/pose 来获取的

x: 1.0 //x坐标
y: 1.0 //y坐标
theta: -1.21437060833 //角度
linear_velocity: 0.0 //线速度
angular_velocity: 1.0 //角速度

2.实现

2.1创建功能包

创建项目功能包依赖于 tf2 tf2_ros tf2_geometry_msgs roscpp rospy std_msgs geometry_msgs turtlesim

2.2服务客户端(生成乌龟)

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

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

    setlocale(LC_ALL,"");

    //执行初始化
    ros::init(argc,argv,"create_turtle");
    //创建节点
    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;
}

此处还有一些细节操作

添加完生成小乌龟程序以后,此时便可以添加launch文件进行调试

<!--
    tf2 实现小乌龟跟随案例
-->
<launch>
    <!-- 启动乌龟节点与键盘控制节点 -->
    <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
    <node pkg="turtlesim" type="turtle_teleop_key" name="key_control" output="screen"/>
    <!-- 启动创建第二只乌龟的节点 -->
    <node pkg="tf04_test" type="Test01_Create_Turtle2" name="turtle2" output="screen" />
    <!-- 启动两个坐标发布节点 -->
    <!-- <node pkg="tf04_test" type="Test02_TF2_Caster" name="caster1" output="screen" args="turtle1" />
    <node pkg="tf04_test" type="Test02_TF2_Caster" name="caster2" output="screen" args="turtle2" /> -->
    <!-- 启动坐标转换节点 -->
    <!-- <node pkg="tf04_test" type="Test03_TF2_Listener" name="listener" output="screen" /> -->
</launch>

运行launch文件可以看到生成两只小乌龟的效果,键盘控制只能控制turtle1,而不能控制我们程序生成的turtle2,因为命名空间不同。

rostopic list

此时可以通过命令行控制turtle2运动

 

2.3发布方(发布两只乌龟的坐标信息)

可以订阅乌龟的位姿信息,然后再转换成坐标信息,两只乌龟的实现逻辑相同,只是订阅的话题名称,生成的坐标信息等稍有差异,可以将差异部分通过参数传入:

  • 该节点需要启动两次
  • 每次启动时都需要传入乌龟节点名称(第一次是 turtle1 第二次是 turtle2)

具体实现的细节在这里

首先是launch文件的不同,注意最后一个参数,args="",这里,这里就是需要传入的参数

    <!-- 启动两个坐标发布节点 -->
    <node pkg="tf04_test" type="Test02_TF2_Caster" name="caster1" output="screen" args="turtle1" />
    <node pkg="tf04_test" type="Test02_TF2_Caster" name="caster2" output="screen" args="turtle2" />

 其次是编程文件里面

多了一段判断的代码,首先是判断 argc 的数值,有传参的话,值等于2,而且传参值是在 argv[1] 里面,可以直接取出来用。

    // 3.解析传入的命名空间
    if (argc != 2)
    {
        ROS_ERROR("请传入正确的参数");
    } else {
        turtle_name = argv[1];
        ROS_INFO("乌龟 %s 坐标发送启动",turtle_name.c_str());
    }

 发布方具体实现

/*  
    该文件实现:需要订阅 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_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;
}

2.4订阅方(解析坐标信息并生成速度信息)

/*  
    订阅 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_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 = 4 * 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;
}

配置文件此处略。

2.5运行

使用 launch 文件组织需要运行的节点,内容示例如下:

<!--
    tf2 实现小乌龟跟随案例
-->
<launch>
    <!-- 启动乌龟节点与键盘控制节点 -->
    <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
    <node pkg="turtlesim" type="turtle_teleop_key" name="key_control" output="screen"/>
    <!-- 启动创建第二只乌龟的节点 -->
    <node pkg="tf04_test" type="Test01_Create_Turtle2" name="turtle2" output="screen" />
    <!-- 启动两个坐标发布节点 -->
    <node pkg="tf04_test" type="Test02_TF2_Caster" name="caster1" output="screen" args="turtle1" />
    <node pkg="tf04_test" type="Test02_TF2_Caster" name="caster2" output="screen" args="turtle2" />
    <!-- 启动坐标转换节点 -->
    <node pkg="tf04_test" type="Test03_TF2_Listener" name="listener" output="screen" />
</launch>

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.coloradmin.cn/o/448144.html

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈,一经查实,立即删除!

相关文章

在外Windows远程连接MongoDB数据库【无公网IP】

文章目录 前言1. 安装数据库2. 内网穿透2.1 安装cpolar内网穿透2.2 创建隧道映射2.3 测试随机公网地址远程连接 3. 配置固定TCP端口地址3.1 保留一个固定的公网TCP端口地址3.2 配置固定公网TCP端口地址3.3 测试固定地址公网远程访问 转载自远程内网穿透的文章&#xff1a;公网远…

【Pytorch学习笔记】12.修改预训练模型权重参数的方法(用于对单通道灰度图使用预训练模型)

文章目录 1.导出模型参数&#xff0c;修改参数2.修改模型结构&#xff0c;导回参数 我们在训练单通道图像&#xff0c;即灰度图&#xff08;如医学影像数据&#xff09;时&#xff0c;常会使用预训练模型进行训练。 但是一般的预训练模型是以ImageNet数据集预训练的&#xff0c…

OpenAI Embedding:快速实现聊天机器人(四)

theme: orange 本文正在参加「金石计划」 接上文OpenAI Embedding&#xff1a;快速实现聊天机器人(三)如何使用Python实现embedding相似度搜索&#xff0c;这篇文章继续讲如何将搜索到的相似文本进行提炼&#xff0c;并最终得出问题的答案。 提炼文本 通过调用azure openai服务…

数据库基础篇 《10.创建和管理表》

1. 基础知识 1.1 一条数据存储的过程 1.2 标识符命名规则 1.3 MySQL中的数据类型 其中&#xff0c;常用的几类类型介绍如下&#xff1a; 2. 创建和管理数据库 2.1 创建数据库 方式1&#xff1a;创建数据库 CREATE DATABASE 数据库名; 方式2&#xff1a;创建数据库并指…

Netty工作模型——网络IO模型的演进,从BIO到NIO到Reactor模型与Netty工作模型

文章目录 一、IO模型1、BIO&#xff08;同步阻塞&#xff09;2、NIO&#xff08;同步非阻塞&#xff09;3、AIO&#xff08;异步非阻塞&#xff09; 二、Reactor模型1、单Reactor单线程2、单Reactor多线程3、主从Reactor多线程 三、Netty工作模型1、Netty工作模型2、Netty入门案…

【Java】『蓝桥杯』10道编程题及答案(三)

系列文章 【Java】『蓝桥杯』10道编程题及答案&#xff08;一&#xff09; 本文链接&#xff1a;https://blog.csdn.net/youcheng_ge/article/details/130223115 【Java】『蓝桥杯』10道编程题及答案&#xff08;二&#xff09; 本文链接&#xff1a;https://blog.csdn.net/y…

智能座舱域

bosch对车载系统的划分&#xff0c;通常分为5大域&#xff1a;动力域&#xff0c;底盘域&#xff0c;车身域&#xff0c;智能座舱域和adas自动驾驶域。随着ECU集成中央化的发展趋势&#xff0c;大众&#xff0c;华为等巨头将动力域&#xff0c;底盘域和车身域合并为整车控制域&…

Java中将json字符串导出为json文件【详细步骤】

一、概述 请根据具体需求具体改动&#xff0c;此代码需要将前端的数据查询出来&#xff0c;然后进行json字符串的转化 .getCatalogId(id)方法是根据id查出来的内容然后再进行转换成json字符串 也可以直接传入json字符串进行测试 二、代码 ApiOperation("导出为json文件&q…

fzyczn生日赛t1 CZN

fzy&czn生日赛t1 CZN 膜拜hybb首杀 文章目录 fzy&czn生日赛t1 CZN题目背景题目描述分析my codewnags code 题目 题目背景 有一天&#xff0c;czn在机房里面心心念念的pj终于来找他了&#xff0c;pj希望czn能够帮助她来解决一道数学题&#xff0c;czn“十分不乐意”地…

数据库基础篇 《8. 聚合函数》

1. 聚合函数介绍 聚合函数不能嵌套调用。比如不能出现类似“AVG(SUM(字段名称))”形式的调用 1.1 AVG和SUM函数 可以对 数值型数据 使用 AVG 和 SUM 函数。 SELECT AVG(salary), MAX(salary),MIN(salary), SUM(salary) FROM employees WHERE job_id LIKE %REP%; 1.2 MIN…

【Latex排版】使用Latex 排版过程中的那些一二三问题汇总

排版错误问题总结&#xff1a; 1.在【\maketitle】 位置处报错----Missing $ inserted. 2.添加参考文献&#xff0c;编译后显示错误&#xff0c;并且pdf中引用文献处为问号(?) 持续更新问题。。。。。。 近期用Latex整理期刊论文时遇到了不少问题&#xff0c;现把遇到的问题及…

2023 最新最细 vite+vue3+ts 多页面项目架构,建议收藏备用!

&#x1f33b; 前言 本文教程 github地址 。 如果对你有帮助&#xff0c;希望能点个star ⭐️⭐️⭐️ 万分感谢&#x1f60a;&#x1f60a;&#x1f60a; &#x1f9f1; 背景 不久前我司需要重新部署一个前端项目&#xff0c;由我来负责这个项目的搭建。因为这个项目是需要…

python爬虫简介

关于爬虫使用 使用python编写的爬虫脚本&#xff08;程序&#xff09;可以完成定时定量&#xff0c;指定目标&#xff08;Web站点&#xff09;的数据爬取&#xff0c;主要使用多&#xff08;单&#xff09;线程/进程&#xff0c;网络请求库&#xff0c;数据解析&#xff0c;数…

记一次误用顶层await导致的路由渲染错误

背景&#xff1a;顶层 await Async 异步函数能将 Promise 的链式调用的形式&#xff0c;改为同步的形式&#xff0c;对于编写和阅读代码都非常友好。但一直以来都有一个限制&#xff0c;就是 async 和 await 这两个关键字必须成对出现。这就导致了一个问题&#xff0c;想使用 …

【JavaScript速成之路】一文带你掌握DOM基础

&#x1f4c3;个人主页&#xff1a;「小杨」的csdn博客 &#x1f525;系列专栏&#xff1a;【JavaScript速成之路】 &#x1f433;希望大家多多支持&#x1f970;一起进步呀&#xff01; 文章目录 前言1&#xff0c;Web API简介1.1&#xff0c;初识Web API1.2&#xff0c;Web A…

TryHackMe-Services(Windows域渗透)

Services 认识团队&#xff01; 今天thm新出的房间&#xff0c;尝尝鲜 端口扫描 循例nmap 把services.local加入hosts Web枚举 发现员工邮箱以及一些员工姓名 从下边的邮箱中&#xff0c;大致可以猜测其他员工账户名跟这个一致的格式 将其保存起来 立足 - AS-REP Roasting…

gin获取url路径参数

package mainimport ("github.com/gin-gonic/gin""net/http" )//获取请求路径的path参数 func main() {r : gin.Default()r.GET("/user/:name/:age", func(c *gin.Context) {//获取路径参数name : c.Param("name")age : c.Param("…

华为云上云实践(一):Windows 环境下对云硬盘 EVS 的创建、挂载和初始化

本文主要讲解华为云云硬盘 EVS 的在 Windows 服务器上创建、挂载及云硬盘初始化等基本操作&#xff0c;快速掌握华为云云硬盘 EVS 操作方法。 文章目录 一、前言二、前期准备&#xff1a;华为云 EVS 采购三、挂载非共享云硬盘 EVS五、初始化云硬盘 EVS 一、前言 华为云 EVS&am…

除了Navicat和DBeaver,还有没有免费又好用的数据库管理工具推荐

最近看到一款数据库Web版工具&#xff0c;SQL Studio&#xff0c;是麦聪软件公司出品的&#xff0c;主打的就是一个&#xff0c;不使用任何的开源代码&#xff0c;产品由中国研发团队100%自主研发。 SQL Studio是一款可创建多个连接的Web版数据库管理开发工具&#xff0c;让你…

RocketMQ的学习历程(二)----MQ基本构架

文章目录 1.MQ的基本要素1.1.消息&#xff08;Message&#xff09;1.2.主题&#xff08;Topic&#xff09;1.3.标签&#xff08;Tag&#xff09;1.4.队列&#xff08;MessageQueue&#xff09;1.5.消息标识&#xff08;MessageId&#xff09; 2.MQ中的主要角色和相关联系2.1.Pr…