ROS学习笔记12——tf坐标变换

以ros自带的小乌龟作为案例的载体，完成通信方式、坐标变换等的练习，实现程序启动之初 产生两只乌龟，中间的乌龟(turtle1) 和 左下乌龟(turtle2), turtle2 会自动运行至turtle1的位置，并且键盘控制时，只是控制 turtle1 的运动，但是 turtle2 可以跟随 turtle1 运行。

1、创建功能包

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

2、服务客户端（生成新的小乌龟turtle2）

#include "ros/ros.h"
#include "turtlesim/Spawn.h"
/*
    需求：向服务端发送请求，生成一只新乌龟
    话题：/spawn -> 通过rosservice list看到
    消息类型：turtlesim/Spawn -> 通过rosservice info /spawn看到
    消息的具体内容：。。。 -> 通过rossrv info turtlesim/Spawn看到
*/

int main(int argc, char *argv[])
{
    /* code */
    setlocale(LC_ALL, "");
    ros::init(argc, argv,"create_new_turtle");
    ros::NodeHandle nh;
    
    //创建服务对象
    ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");

    //组织数据
    turtlesim::Spawn spawn;
    spawn.request.name = "turtle2";
    spawn.request.x = 1.0;
    spawn.request.y = 2.0;
    spawn.request.theta = 1.57;
    client.waitForExistence();//判断服务器状态

    //发送请求
    bool flag = client.call(spawn);//flag用来接收响应状态的，响应结果也会被设置进spawn对象
    if(flag)
    {
        ROS_INFO("新乌龟 %s 成功生成", spawn.response.name.c_str());
    }
    else{
        ROS_INFO("新乌龟生成失败");
    }
    ros::spin();
    return 0;
}

3、发布方（发布两只小乌龟的坐标信息）

可以订阅乌龟的位姿信息，然后转换成坐标系关系，两只乌龟的实现逻辑是相同的，修改订阅话题和消息类型之后，新建一个cpp重复实现一遍逻辑当然是可以的，但是加大了代码量，并且降低了代码的复用率。这些细微的差异可以通过args设置参数来传入：

  • 该节点需要启动两次
  • 每次启动时都需要传入乌龟节点名称(第一次是 turtle1 第二次是 turtle2)
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
/*
发布方实现
    需求：订阅乌龟的位姿信息，转换成相对于窗体的坐标关系，并发布
    准备：
        话题：/turtle1/pose、/turtle2/pose
        消息：turtlesim/Pose
    
    流程：
        1、包含头文件
        2、设置编码、初始化、NodeHandle等
        3、创建订阅对象、订阅话题：/turtle1/pose、/turtle2/pose
        4、**回调函数，处理订阅的消息：把订阅的位姿信息转换成坐标系相对关系，并发布
        5、spin()
*/

//声明接收传递参数的变量
std::string turtle_name;

void doPose(const turtlesim::Pose::ConstPtr &pose)
{
    //获取位姿信息，转换成坐标系相对关系，并发布
    //5.1 创建发布对象
    static tf2_ros::TransformBroadcaster pub;   //设为static静态变量，每次回调使用同一个pub对象
    //5.2 组织被发布的数据  
    geometry_msgs::TransformStamped ts;
    ts.header.frame_id = "world";
    ts.header.stamp = ros::Time::now();
    //**子级坐标系名称动态传入
    ts.child_frame_id = turtle_name;
    //坐标系偏移量
    ts.transform.translation.x = pose->x;
    ts.transform.translation.y = pose->y;
    ts.transform.translation.z = 0;

    tf2::Quaternion qnt;
    qnt.setRPY(0,0,pose->theta);
    ts.transform.rotation.w = qnt.getW();
    ts.transform.rotation.x = qnt.getX();
    ts.transform.rotation.y = qnt.getY();
    ts.transform.rotation.z = qnt.getZ();
    
    //5.3 发布
    pub.sendTransform(ts);
}

int main(int argc, char *argv[])
{
    /* code */
    /*
        解析 launch 文件通过 args 传入的参数
    */
    if(argc != 4)
    {
        ROS_INFO("请传入一个参数");
        return 1;
    }else
    {
        turtle_name = argv[1];
    }

    setlocale(LC_ALL,"");
    // 2、设置编码、初始化、NodeHandle等
    ros::init(argc,argv,"dynamic_pub_turtle");
    ros::NodeHandle nh;
    // **3、创建订阅对象、订阅话题：/turtle1 或者 turtle2 是动态传入的
    ros::Subscriber sub = nh.subscribe<turtlesim::Pose>(turtle_name + "/pose",100,doPose);
    // 4、**回调函数，处理订阅的消息：把订阅的位姿信息转换成坐标相对关系，并发布
    // 5、spin()    
    ros::spin();
    return 0;
}

4、订阅方（解析坐标信息并且生成速度信息）

#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"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/Twist.h"
/*
    需求1：换算出 turtle1 相对于 turtle2 的关系
    需求2：计算角速度和线速度并发布
*/
int main(int argc, char *argv[])
{
    /* code */
    setlocale(LC_ALL, "");
    // 2、编码、初始化、NodeHandle
    ros::init(argc, argv,"tfs_sub");
    ros::NodeHandle nh;
    // 3、创建订阅对象
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener sub(buffer);

    //A、创建发布对象
    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",100);

    // 4、编写解析逻辑（循环，坐标关系和坐标点的转换）
    //ros::Duration(2).sleep();
    ros::Rate rate(10);
    while(ros::ok())
    {

        try
        {
        // １、计算 turtle1 和 turtle2 之间的相对关系
        /* lookupTransform(参数1：B，参数2：A，参数3)
            ## A 相对于 B 的坐标系关系
            参数1："目标坐标系 B"
            参数2："源坐标系 A"
            参数3：ros::Time(0) 取间隔最短的两个坐标系帧计算相对关系
            返回值：geometry_msgs::TransformStamped
        */
        geometry_msgs::TransformStamped tfs = buffer.lookupTransform("turtle2","turtle1",ros::Time(0));
        ROS_INFO("turtle1 相对于 turtle2 的信息：父级名称:%s,子级名称:%s,偏移量:(%.2f,%.2f,%.2f)",
                    tfs.header.frame_id.c_str(), //turtle2
                    tfs.child_frame_id.c_str(),   //turtle1
                    tfs.transform.translation.x,
                    tfs.transform.translation.y,
                    tfs.transform.translation.z
                    ); 
        //B、根据相对位置计算并组织速度消息
        geometry_msgs::Twist twist;
        /*
           linear.x = 系数 * 开放(y^2 + x^2)
           angular.z = 反正切(对边，临边)
        */
        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);

        //C、发布
        pub.publish(twist);

        }
        catch(const std::exception& e)
        {
            //std::cerr << e.what() << '\n';
            ROS_INFO("异常消息：%s", e.what());
        }
        
        rate.sleep();
        // 5、spinOnce()
        ros::spinOnce();
    }
    return 0;
}

5、运行

使用 launch 文件组织需要运行的节点

<launch>
    <!-- 启动小乌龟GUI -->
    <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
    <!-- 键盘控制节点 -->
    <node pkg="turtlesim" type="turtle_teleop_key" name="key" output="screen" />
    <!-- 创建新的小乌龟 -->
    <node pkg="tfs_test" type="create_new_turtle" name="turtle2" output="screen" />

    <!-- 需要启动两个乌龟相对于世界坐标系的坐标关系的发布节点 -->
    <!-- 实现思路：
            1、只编写一个节点
            2、这个节点需要启动两次
            3、节点启动时，动态传参：
                        第一次：turtle1
                        第二次：turtle2
     -->
    <node pkg="tfs_test" type="pub_turtle" name="pub_turtle1" args="turtle1" output="screen" />
    <node pkg="tfs_test" type="pub_turtle" name="pub_turtle2" args="turtle2" output="screen" />
    
    <!-- 订阅turtle1与 turtle2 相对于世界坐标系的坐标关系
         并生成 turtle1 相对于 turtle2 的坐标关系
         再生成速度消息 -->
    <node pkg="tfs_test" type="control_turtle2" name="control" output="screen" />
</launch>

6、记得配置CMakeLists

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

如若内容造成侵权/违法违规/事实不符，请联系我们进行投诉反馈qq邮箱809451989@qq.com，一经查实，立即删除！

相关文章

代码随想录刷题随记29-贪心3

代码随想录刷题随记29-贪心3

代码随想录刷题随记29-贪心3 1005.K次取反后最大化的数组和 leetcode链接 比较简单&#xff0c;首先对数组进行绝对值排序&#xff0c;然后如果是负数从小到大进行反转 如果是正数&#xff0c;就对一个绝对值最小的一直翻转 按照绝对值排序的实现可以通过重写比较器实现 cla…
阅读更多...
ComfyUI-AniPortrait——数字人插件

ComfyUI-AniPortrait——数字人插件

仓库地址&#xff1a;GitHub - chaojie/ComfyUI-AniPortrait 往期学习资料 整理AI学习资料库 需要的模型如下 工作流如下&#xff1a; 首先把上面的sd-vae-ft-mse、wav2vec2-base-960h模型放到下面的目录&#xff0c;如下 其他模型放到哪里都行&#xff0c;反正是自定义模型…
阅读更多...
ThreeJs模拟工厂生产过程八

ThreeJs模拟工厂生产过程八

这节算是给这个车间场景收个尾&#xff0c;等了几天并没有人发设备模型给我&#xff0c;只能自己找了一个凑合用了。加载模型之前&#xff0c;首先要把货架上的料箱合并&#xff0c;以防加载模型之后因模型数量多出现卡顿&#xff0c;方法和之前介绍的合并传送带方法相同&#…
阅读更多...
uniapp视频播放器（h5+app）

uniapp视频播放器（h5+app）

关于uniapp视频播放器遇到的一些问题&#xff0c;mark下。 中途遇到了很多问题&#xff0c;如果有相同的伙伴遇到了类似的&#xff0c;欢迎交流 官方的video播放器在app上不友好&#xff0c;有以下功能不支持。 loadedmetadata、controlstoggle不支持导致只能手写控制层。 不…
阅读更多...
集成框架 -- OSS

集成框架 -- OSS

前言 接入oss必须有这两个文档基础 使用STS临时访问凭证访问OSS_对象存储(OSS)-阿里云帮助中心 前端上传跨域 正文 sts前后端通用&#xff0c;开通图示 AliyunSTSAssumeRoleAccess 后端实现代码 public static void main(String[] args) {String regionId "cn-ha…
阅读更多...
Oracle 表分区

Oracle 表分区

1.概述 分区表就是将表在物理存储层面分成多个小的片段&#xff0c;这些片段即称为分区&#xff0c;每个分区保存表的一部分数据&#xff0c;表的分区对上层应用是完全透明的&#xff0c;从应用的角度来看&#xff0c;表在逻辑上依然是一个整体。 目的&#xff1a;提高大表的查…
阅读更多...
2024年北京市中小学生信息学能力测评活动BCSP-X小学低年级组初赛测试题（模拟题）

2024年北京市中小学生信息学能力测评活动BCSP-X小学低年级组初赛测试题（模拟题）

一、单项选择&#xff08;共 15 题&#xff0c;每题 2 分&#xff0c;共计 30 分&#xff0c;每题有且仅有一个正确选项&#xff09; 不可以作为c中的变量名的是&#xff08; &#xff09;。 A. I以下loveChinaB. I_loveChinaC. I_love_ChinaD. i_loveChina 在体育课上&#xf…
阅读更多...
teamOS协作通知，我的新晋办公搭子，完美把控项目动态，再也不担心错过协作变更了，谁也不能背着我偷偷内卷

teamOS协作通知，我的新晋办公搭子，完美把控项目动态，再也不担心错过协作变更了，谁也不能背着我偷偷内卷

有没有碰到过这样的情况&#xff0c;在企业网盘中建了新项目的协作组&#xff0c;和团队成员一起做项目&#xff0c;正常来说应该是能更好的完成工作。 但是现实就是&#xff0c;项目文件修改了&#xff0c;如果不用微信或者其他方式发个通知&#xff0c;团队成员往往都不知道…
阅读更多...
selenium 4.x 入门（环境搭建、八大元素定位）

selenium 4.x 入门（环境搭建、八大元素定位）

背景 Web自动化测现状 1. 属于 E2E 测试 2. 过去通过点点点 3. 好的测试&#xff0c;还需要记录、调试网页的细节 一、selenium4.x环境搭建 一键搭建 pip3 install webdriver-helper 有建议要 1.0.1 版本的&#xff0c;但本人按上面的是可以正常使用&#xff08;看…
阅读更多...
计算机科学与技术就业方向和前景怎么样

计算机科学与技术就业方向和前景怎么样

计算机科学与技术专业的就业方向极为广泛&#xff0c;方向可以是软件开发与工程、网络与信息安全、数据科学与大数据分析等&#xff0c;几乎渗透到现代社会的每一个角落。以下是上大学网 &#xff08;www.sdaxue.com)对计算机科学与技术专业一些主要的就业方向及其前景分析&…
阅读更多...
【Redis 开发】Redis哨兵

【Redis 开发】Redis哨兵

哨兵 作用和原理服务状态监控选举新的master 搭建哨兵集群RedisTemplate的哨兵模式 作用和原理 Redis提供了哨兵机制来实现主从集群中的自动故障恢复&#xff1a; 哨兵也是一个集群 监控&#xff1a;会不断检查master和slave是否按预期工作自动故障恢复&#xff1a;如果mast…
阅读更多...
基于FPGA的数字信号处理（2）--什么是定点数？

基于FPGA的数字信号处理（2）--什么是定点数？

在实际的工程应用中&#xff0c;往往会进行大量的数学运算。运算时除了会用到整数&#xff0c;很多时候也会用到小数。而我们知道在数字电路底层&#xff0c;只有「高电平1」和「低电平0」的存在&#xff0c;那么仅凭 0和1 该如何表示小数呢&#xff1f; 数字电路中&#xff0…
阅读更多...
SpringBoot实现图片上传（个人头像的修改）

SpringBoot实现图片上传（个人头像的修改）

SpringBootlayui实现个人信息头像的更改 该文章适合对SpringBoot&#xff0c;Thymeleaf&#xff0c;layui入门的小伙伴 废话不多说&#xff0c;直接上干货 Springbootlayui实现头像更换 前端公共部分代码 HTML页面代码 <div class"layui-card-header" style&quo…
阅读更多...
IP定位技术企业网络安全检测

IP定位技术企业网络安全检测

随着信息技术的飞速发展&#xff0c;网络安全问题日益凸显&#xff0c;成为企业运营中不可忽视的一环。在众多网络安全技术中&#xff0c;IP定位技术以其独特的优势&#xff0c;为企业网络安全检测提供了强有力的支持。本文将深入探讨IP定位技术在企业网络安全检测中的应用及其…
阅读更多...
QT学习之读取xml中信息

QT学习之读取xml中信息

背景&#xff1a; 我们每次注册后会生成对应的启动码文件&#xff0c;格式如下&#xff0c;启动码最后要在测试工具使用的进行一个验证&#xff0c;验证通过后模块才能使用。所以我希望每次的xml都放在一个文件夹里&#xff0c;等我选择文件夹后&#xff0c;能提取所有xml中的对…
阅读更多...
手把手教会西门子PLC代码可视化功能——Prodiag

手把手教会西门子PLC代码可视化功能——Prodiag

一、传统的HMI报警方法 在HMI中建立离散量报警&#xff0c;输入报警文本。这种方法的劣势&#xff1a; 1、在PLC程序中需要建立专门报警程序&#xff0c;用于关联HMI中的报警变量 2、需要在HMI文本中输入报警文本 如果程序复杂&#xff0c;报警众多&#xff0c;用这种方法需…
阅读更多...
性能监控之prometheus+grafana搭建

性能监控之prometheus+grafana搭建

前言 Prometheus和Grafana是两个流行的开源工具&#xff0c;用于监控和可视化系统和应用程序的性能指标。它们通常一起使用&#xff0c;提供了强大的监控和数据可视化功能。 Prometheus Prometheus是一种开源的系统监控和警报工具包。它最初由SoundCloud开发&#xff0c;并于…
阅读更多...
二分查找法实例

二分查找法实例

本文是根据数据结构中常常提到的二分法而作的一篇博客&#xff0c;主要通过一个二分法实例进行展开说明&#xff1a; 实例内容 通过一个二分法函数来寻找某个数是否在给定的数组中&#xff1b; 代码展示 # 执行二分查找法的算法函数 # 二分法查找的对象必须是一个有序的集…
阅读更多...
尼日利亚光伏储能展

尼日利亚光伏储能展

尼日利亚地处热带地区&#xff0c;全年阳光充足&#xff0c;每年日照时间超2600小时(平均每天约7小时)。专家表示&#xff0c;尼日利亚有足够的经济实力和环境条件来开发可再生能源&#xff0c;尤其是太阳能。据世界银行估计&#xff0c;投资太阳能发电厂可为近8000万人提供电力…
阅读更多...
短视频交友系统搭建重点，会用到哪些三方服务？

短视频交友系统搭建重点，会用到哪些三方服务？

在搭建短视频交友系统时&#xff0c;为了确保系统的稳定性、安全性和用户体验&#xff0c;通常需要用到多种第三方服务。以下是搭建短视频交友系统时可能用到的关键第三方服务&#xff1a; 云服务提供商&#xff1a;如阿里云、腾讯云等&#xff0c;提供稳定、可扩展的服务器资源…
阅读更多...
最新文章

Copyright @ 2022~2023

友情链接： 我的编程经验分享 一条长河网 尧图网站开发 尧图建网站