第 5 章 ROS常用组件动态坐标变换(自学二刷笔记)

news2024/12/27 10:18:16

5.1.3 动态坐标变换

所谓动态坐标变换，是指两个坐标系之间的相对位置是变化的。

需求描述:

启动 turtlesim_node，该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟运动，将两个坐标系的相对位置动态发布。

结果演示:

实现分析:

  1. 乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点

  2. 订阅 turtle1/pose，可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度

  3. 将 pose 信息转换成 坐标系相对信息 并发布

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

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

  2. 创建坐标相对关系发布方(同时需要订阅乌龟位姿信息)

  3. 创建坐标相对关系订阅方

  4. 执行

方案A：C++实现

1.创建功能包

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

2.发布方
/*  
    动态的坐标系相对姿态发布(一个坐标系相对于另一个坐标系的相对姿态是不断变动的)

    需求: 启动 turtlesim_node，该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘
    控制乌龟运动，将两个坐标系的相对位置动态发布

    实现分析:
        1.乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点
        2.订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度
        3.将 pose 信息转换成 坐标系相对信息并发布

    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建 ROS 句柄
        4.创建订阅对象
        5.回调函数处理订阅到的数据(实现TF广播)
            5-1.创建 TF 广播器
            5-2.创建 广播的数据(通过 pose 设置)
            5-3.广播器发布数据
        6.spin
*/
// 1.包含头文件
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

void doPose(const turtlesim::Pose::ConstPtr& pose){
    //  5-1.创建 TF 广播器
    static tf2_ros::TransformBroadcaster broadcaster;
    //  5-2.创建 广播的数据(通过 pose 设置)
    geometry_msgs::TransformStamped tfs;
    //  |----头设置
    tfs.header.frame_id = "world";
    tfs.header.stamp = ros::Time::now();

    //  |----坐标系 ID
    tfs.child_frame_id = "turtle1";

    //  |----坐标系相对信息设置
    tfs.transform.translation.x = pose->x;
    tfs.transform.translation.y = pose->y;
    tfs.transform.translation.z = 0.0; // 二维实现，pose 中没有z，z 是 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();


    //  5-3.广播器发布数据
    broadcaster.sendTransform(tfs);
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"dynamic_tf_pub");
    // 3.创建 ROS 句柄
    ros::NodeHandle nh;
    // 4.创建订阅对象
    ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose",1000,doPose);
    //     5.回调函数处理订阅到的数据(实现TF广播)
    //        
    // 6.spin
    ros::spin();
    return 0;
}

配置文件此处略。

3.订阅方
//1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h" //注意: 调用 transform 必须包含该头文件

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"dynamic_tf_sub");
    ros::NodeHandle nh;
    // 3.创建 TF 订阅节点
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);

    ros::Rate r(1);
    while (ros::ok())
    {
    // 4.生成一个坐标点(相对于子级坐标系)
        geometry_msgs::PointStamped point_laser;
        point_laser.header.frame_id = "turtle1";
        point_laser.header.stamp = ros::Time();
        point_laser.point.x = 1;
        point_laser.point.y = 1;
        point_laser.point.z = 0;
    // 5.转换坐标点(相对于父级坐标系)
        //新建一个坐标点，用于接收转换结果  
        //--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------
        try
        {
            geometry_msgs::PointStamped point_base;
            point_base = buffer.transform(point_laser,"world");
            ROS_INFO("坐标点相对于 world 的坐标为:(%.2f,%.2f,%.2f)",point_base.point.x,point_base.point.y,point_base.point.z);

        }
        catch(const std::exception& e)
        {
            // std::cerr << e.what() << '\n';
            ROS_INFO("程序异常:%s",e.what());
        }


        r.sleep();  
        ros::spinOnce();
    }


    return 0;
}

配置文件此处略。

4.执行

可以使用命令行或launch文件的方式分别启动发布节点与订阅节点，如果程序无异常，与演示结果类似。

可以使用 rviz 查看坐标系相对关系。

方案B：Python实现

1.创建功能包

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

2.发布方
#! /usr/bin/env python
"""  
    动态的坐标系相对姿态发布(一个坐标系相对于另一个坐标系的相对姿态是不断变动的)

    需求: 启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘
    控制乌龟运动，将两个坐标系的相对位置动态发布

    实现分析:
        1.乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点
        2.订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度
        3.将 pose 信息转换成 坐标系相对信息并发布
    实现流程:
        1.导包
        2.初始化 ROS 节点
        3.订阅 /turtle1/pose 话题消息
        4.回调函数处理
            4-1.创建 TF 广播器
            4-2.创建 广播的数据(通过 pose 设置)
            4-3.广播器发布数据
        5.spin
"""
# 1.导包
import rospy
import tf2_ros
import tf
from turtlesim.msg import Pose
from geometry_msgs.msg import TransformStamped

#     4.回调函数处理
def doPose(pose):
    #         4-1.创建 TF 广播器
    broadcaster = tf2_ros.TransformBroadcaster()
    #         4-2.创建 广播的数据(通过 pose 设置)
    tfs = TransformStamped()
    tfs.header.frame_id = "world"
    tfs.header.stamp = rospy.Time.now()
    tfs.child_frame_id = "turtle1"
    tfs.transform.translation.x = pose.x
    tfs.transform.translation.y = pose.y
    tfs.transform.translation.z = 0.0
    qtn = tf.transformations.quaternion_from_euler(0,0,pose.theta)
    tfs.transform.rotation.x = qtn[0]
    tfs.transform.rotation.y = qtn[1]
    tfs.transform.rotation.z = qtn[2]
    tfs.transform.rotation.w = qtn[3]
    #         4-3.广播器发布数据
    broadcaster.sendTransform(tfs)

if __name__ == "__main__":
    # 2.初始化 ROS 节点
    rospy.init_node("dynamic_tf_pub_p")
    # 3.订阅 /turtle1/pose 话题消息
    sub = rospy.Subscriber("/turtle1/pose",Pose,doPose)
    #     4.回调函数处理
    #         4-1.创建 TF 广播器
    #         4-2.创建 广播的数据(通过 pose 设置)
    #         4-3.广播器发布数据
    #     5.spin
    rospy.spin()

权限设置以及配置文件此处略。

3.订阅方
#! /usr/bin/env python
"""  
    动态的坐标系相对姿态发布(一个坐标系相对于另一个坐标系的相对姿态是不断变动的)

    需求: 启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘
    控制乌龟运动，将两个坐标系的相对位置动态发布

    实现分析:
        1.乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点
        2.订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度
        3.将 pose 信息转换成 坐标系相对信息并发布
    实现流程:
        1.导包
        2.初始化 ROS 节点
        3.创建 TF 订阅对象
        4.处理订阅的数据


"""
# 1.导包
import rospy
import tf2_ros
# 不要使用 geometry_msgs,需要使用 tf2 内置的消息类型
from tf2_geometry_msgs import PointStamped
# from geometry_msgs.msg import PointStamped

if __name__ == "__main__":
    # 2.初始化 ROS 节点
    rospy.init_node("static_sub_tf_p")
    # 3.创建 TF 订阅对象
    buffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(buffer)

    rate = rospy.Rate(1)
    while not rospy.is_shutdown():    
    # 4.创建一个 radar 坐标系中的坐标点
        point_source = PointStamped()
        point_source.header.frame_id = "turtle1"
        point_source.header.stamp = rospy.Time.now()
        point_source.point.x = 10
        point_source.point.y = 2
        point_source.point.z = 3

        try:
    #     5.调研订阅对象的 API 将 4 中的点坐标转换成相对于 world 的坐标
            point_target = buffer.transform(point_source,"world",rospy.Duration(1))
            rospy.loginfo("转换结果:x = %.2f, y = %.2f, z = %.2f",
                            point_target.point.x,
                            point_target.point.y,
                            point_target.point.z)
        except Exception as e:
            rospy.logerr("异常:%s",e)

    #     6.spin
        rate.sleep()

权限设置以及配置文件此处略。

4.执行

可以使用命令行或launch文件的方式分别启动发布节点与订阅节点，如果程序无异常，与演示结果类似。

可以使用 rviz 查看坐标系相对关系。

另请参考:

  • tf2/Tutorials/Writing a tf2 broadcaster (C++) - ROS Wiki
  • tf2/Tutorials/Writing a tf2 broadcaster (Python) - ROS Wiki

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

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

相关文章

请说明Vue中的异步组件加载

请说明Vue中的异步组件加载

Vue中的异步组件加载是指当页面需要渲染某个组件时&#xff0c;可以在需要时再去加载这个组件&#xff0c;而不是在页面初始化的时候就将所有组件一次性加载进来。这种方式能够有效降低页面的初始加载时间&#xff0c;提升用户体验。 在Vue中&#xff0c;我们可以使用import函…
阅读更多...
idea Gradle 控制台中文乱码

idea Gradle 控制台中文乱码

如下图所示&#xff0c;idea 中的 Gradle 控制台中文乱码&#xff1a; 解决方法&#xff0c;如下图所示&#xff1a; 注意&#xff1a;如果你的 idea 使用 crack 等方式破解了&#xff0c;那么你可能需要在文件 crack-2023\jetbra\vmoptions\idea.vmoptions 中进行配置&#xf…
阅读更多...
每日五道java面试题之springMVC篇（一）

每日五道java面试题之springMVC篇（一）

目录&#xff1a; 第一题. 什么是Spring MVC&#xff1f;简单介绍下你对Spring MVC的理解&#xff1f;第二题. Spring MVC的优点第三题. Spring MVC的主要组件&#xff1f;第四题. 什么是DispatcherServlet?第五题. 什么是Spring MVC框架的控制器&#xff1f; 第一题. 什么是S…
阅读更多...
基于51单片机的车辆出入光电传感管理系统设计

基于51单片机的车辆出入光电传感管理系统设计

目 录 摘 要 I Abstract II 引 言 1 第1章 系统总体方案设计 3 1.1 整体方案设计 3 1.1.1 控制系统的选择 3 1.1.2 系统框图 4 1.2 最小系统模块 4 1.2.1 STC89C51简介 4 1.2.2 最小系统电路 6 第2章 系统硬件设计 8 2.1 单片机的选择 8 2.2 显示方案的选择 8 2.3 红外检测模块…
阅读更多...
VisionPro 判断圆是不是无限接近圆或存在缺陷

VisionPro 判断圆是不是无限接近圆或存在缺陷

项目上可能需要判断圆是否是无限接近圆或者判断圆边缘是否存在缺陷等。 第一种方法&#xff1a;找圆工具和点到点的距离计算圆边缘上的点到圆心距离的最大值和最小值的差值。 #region namespace imports using System; using System.Collections; using System.Drawing; usin…
阅读更多...
【论文精读】大语言模型融合知识图谱的问答系统研究

【论文精读】大语言模型融合知识图谱的问答系统研究

&#x1f497;&#x1f497;&#x1f497;欢迎来到我的博客&#xff0c;你将找到有关如何使用技术解决问题的文章&#xff0c;也会找到某个技术的学习路线。无论你是何种职业&#xff0c;我都希望我的博客对你有所帮助。最后不要忘记订阅我的博客以获取最新文章&#xff0c;也欢…
阅读更多...
安装sqlserver2022最新版只能使用.\SQLEXPRESS登录数据库怎么修改成.

安装sqlserver2022最新版只能使用.\SQLEXPRESS登录数据库怎么修改成.

.\SQLEXPRESS “服务器名称 localhost\SQLEXPRESS”中的 “SQLEXPRESS”就是数据库的实例名称/数据库名/服务器名&#xff0c; “localhost”即登录本计算机安装的数据库 安装sqlserver2022最新版只能使用.\SQLEXPRESS登录数据库怎么修改成. 2、查看SQL Server数据库的实例名…
阅读更多...
Myqsort：基于冒泡排序算法的C语言实现

Myqsort：基于冒泡排序算法的C语言实现

我们将详细介绍一个基于冒泡排序算法的自定义排序函数——Mysqrt。该函数通过使用用户提供的比较函数进行元素间的比较&#xff0c;并结合swap交换函数对任意类型的数据进行排序。下面是对代码的逐行解析。 逻辑导图 代码实现 // 头文件 #include<stdio.h>// 定义比较函…
阅读更多...
RK3568平台 USB数据包的收发格式

RK3568平台 USB数据包的收发格式

一.USB硬件拓扑结构 compound device &#xff1a;多个设备组合起来&#xff0c;通过HUB跟Host相连composite device &#xff1a;一个物理设备有多个逻辑设备(multiple interfaces) 在软件开发过程中&#xff0c;我们可以忽略Hub的存在&#xff0c;硬件拓扑图简化如下&#x…
阅读更多...
事务失效的八种情况！！！！

事务失效的八种情况！！！！

一、非publi修饰的方法。 /*** 私有方法上的注解&#xff0c;不生效&#xff08;因私有方法Spring扫描不到该方法&#xff0c;所以无法生成代理&#xff09;*/ Transactional private boolean test() {//test code }二、类内部访问。 类内部非直接访问带注解标记的方法 B&…
阅读更多...
php：下拉列表查询（静态数据+数据库数据）

php：下拉列表查询（静态数据+数据库数据）

一、在php中嵌套 效果 1、从php中嵌套html语句 下拉列表的显示 echo <div class"text-nav-1 required "><div> . _(在职状态) . :</div> <select name"work_status">; // 定义选项数组 $options [all > _(全部),inwork &g…
阅读更多...
新型设备巡检方案-手机云巡检

新型设备巡检方案-手机云巡检

随着科技的不断发展&#xff0c;设备巡检工作也在逐步向智能化、高效化方向转变。传统的巡检方式往往需要人工逐个设备检查&#xff0c;耗时耗力&#xff0c;效率低下&#xff0c;同时还容易漏检和误检。而新型设备巡检应用—手机蓝牙云巡检的出现&#xff0c;则为设备巡检工作…
阅读更多...
基于springboot+vue的高校教师电子名片系统

基于springboot+vue的高校教师电子名片系统

博主主页&#xff1a;猫头鹰源码 博主简介&#xff1a;Java领域优质创作者、CSDN博客专家、阿里云专家博主、公司架构师、全网粉丝5万、专注Java技术领域和毕业设计项目实战&#xff0c;欢迎高校老师\讲师\同行交流合作 ​主要内容&#xff1a;毕业设计(Javaweb项目|小程序|Pyt…
阅读更多...
2024年腾讯云学生服务器活动详细说明、学生机购买流程

2024年腾讯云学生服务器活动详细说明、学生机购买流程

2024年腾讯云学生服务器优惠活动「云校园」&#xff0c;学生服务器优惠价格&#xff1a;轻量应用服务器2核2G学生价30元3个月、58元6个月、112元一年&#xff0c;轻量应用服务器4核8G配置191.1元3个月、352.8元6个月、646.8元一年&#xff0c;CVM云服务器2核4G配置842.4元一年&…
阅读更多...
2024年最佳硬盘！为台式电脑、NAS等产品量身定做的顶级机械硬盘

2024年最佳硬盘！为台式电脑、NAS等产品量身定做的顶级机械硬盘

机械硬盘&#xff08;HDD&#xff09;可能看起来像是古老的技术&#xff0c;但它们仍然在许多地方提供“足够好”的性能&#xff0c;并且它们很容易以同等的价格提供最多的存储空间。 尽管最好的SSD将为你的操作系统和引导驱动器提供最好的体验&#xff0c;并提供比HDD更好的应…
阅读更多...
分布式商业生态战略是数字化时代企业低成本、高效率最新发展方案

分布式商业生态战略是数字化时代企业低成本、高效率最新发展方案

企业存在的本质是解决市场难以解决的或即使可以解决也需要耗费大量人力、物力的问题&#xff0c;并以其特有的资源能力降低交易费用&#xff0c;实现盈利。从交易费用角度来说&#xff0c;交易费用决定了企业的存在&#xff0c;企业采取不同的组织方式主要是为了对资源采取不同…
阅读更多...
分布式搜索引擎-elasticsearch基础

分布式搜索引擎-elasticsearch基础

分布式搜索引擎-elasticsearch基础 1、什么是elasticsearch&#xff1f; elasticsearch是一款非常强大的开源搜索引擎&#xff0c;可以帮助我们从海量数据中快速找到需要的内容。 elasticsearch结合kibana、Logstash、Beats&#xff0c;也就是elastic stack&#xff08;ELK&a…
阅读更多...
lanqiao:合根植物

lanqiao:合根植物

题目描述&#xff1a; 代码实现&#xff1a;
阅读更多...
AD20软件使用指南：拼板操作与Gerber文件生成详解

AD20软件使用指南：拼板操作与Gerber文件生成详解

文章目录 一、前言二、拼板1.创建新的PCB&#xff0c;用于放置拼板文件2.放置拼板阵列3.设置阵列信息4.V割拼板&#xff0c;放置工艺边和定位孔和光点5.完成拼板 三、生成Gerber文件1.输出Gerber文件2.选择单位和格式3.选择输出的图层4.生成Gerber文件5.生成钻孔文件 四、上传嘉…
阅读更多...
element-plus upload上传

element-plus upload上传

vue3 element-plus 2.4.3 <script> const submitUpload () > {// 单文件proxy.$refs["uploadRef"].submit(); }const uploadFile (options) > {let formData new FormData();formData.append("file", options.file)return uploadFileWith…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023