【ROS】—— ROS常用组件_TF坐标变换_静态坐标变换与动态坐标变换(十)

news2024/9/27 21:18:35

文章目录

  • 前言
    • 1. 坐标msg消息
    • 1.1 geometry_msgs/TransformStamped
    • 1.2 geometry_msgs/PointStamped
  • 2. 静态坐标变换
    • 2.1 C++实现
      • 2.1.1 发布方
      • 2.1.2 订阅方
    • 2.2 python实现
      • 2.2.1 发布方
      • 2.2.2 订阅方
    • 2.3 补充
  • 3. 动态坐标变换
    • 3.1 C++实现
      • 3.1.1 发布方
      • 3.1.2 订阅方
    • 3.2 python实现
      • 3.2.1 发布方
      • 3.2.2 订阅方

前言

📢本系列将依托赵虚左老师的ROS课程，写下自己的一些心得与笔记。
📢课程链接:https://www.bilibili.com/video/BV1Ci4y1L7ZZ
📢讲义链接:http://www.autolabor.com.cn/book/ROSTutorials/index.html
📢 文章可能存在疏漏的地方，恳请大家指出。

ROS中内置有小乌龟跟随实践案例,输入以下命令

roslaunch turtle_tf2 turtle_tf2_demo_cpp.launch或roslaunch turtle_tf2 turtle_tf2_demo.launch

带cpp的是用C++实现的,另一个则是python实现的.
在这里插入图片描述
概念
tf: TransForm Frame,坐标变换
坐标系: ROS 中是通过坐标系统开标定物体的，确切的将是通过右手坐标系来标定的。

作用
在 ROS 中用于实现不同坐标系之间的点或向量的转换。

说明

在ROS中坐标变换最初对应的是tf，不过在 hydro 版本开始, tf 被弃用，迁移到 tf2,后者更为简洁高效，tf2对应的常用功能包有:

tf2_geometry_msgs: 可以将ROS消息转换成tf2消息。
tf2: 封装了坐标变换的常用消息。
tf2_ros: 为tf2提供了roscpp和rospy绑定，封装了坐标变换常用的API。

1. 坐标msg消息

订阅发布模型中数据载体 msg 是一个重要实现，首先需要了解一下，在坐标转换实现中常用的 msg:geometry_msgs/TransformStamped和geometry_msgs/PointStamped前者用于传输坐标系相关位置信息，后者用于传输某个坐标系内坐标点的信息。在坐标变换中，频繁的需要使用到坐标系的相对关系以及坐标点信息。

1.1 geometry_msgs/TransformStamped

命令行键入:rosmsg info geometry_msgs/TransformStamped

std_msgs/Header header                     #头信息
  uint32 seq                                #|-- 序列号
  time stamp                                #|-- 时间戳
  string frame_id                            #|-- 坐标 ID
string child_frame_id                    #子坐标系的 id
geometry_msgs/Transform transform        #坐标信息
  geometry_msgs/Vector3 translation        #偏移量
    float64 x                                #|-- X 方向的偏移量
    float64 y                                #|-- Y 方向的偏移量
    float64 z                                #|-- Z 方向上的偏移量
  geometry_msgs/Quaternion rotation        #四元数
    float64 x                                
    float64 y                                
    float64 z                                
    float64 w

1.2 geometry_msgs/PointStamped

命令行键入:rosmsg info geometry_msgs/PointStamped

std_msgs/Header header                    #头
  uint32 seq                                #|-- 序号(一般不需要关注)
  time stamp                                #|-- 时间戳
  string frame_id                            #|-- 所属坐标系的 id
geometry_msgs/Point point                #点坐标
  float64 x                                    #|-- x y z 坐标
  float64 y
  float64 z

2. 静态坐标变换

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

需求描述:

现有一机器人模型，核心构成包含主体与雷达，各对应一坐标系，坐标系的原点分别位于主体与雷达的物理中心，已知雷达原点相对于主体原点位移关系如下: x 0.2 y0.0 z0.5。当前雷达检测到一障碍物，在雷达坐标系中障碍物的坐标为 (2.0 3.0 5.0),请问，该障碍物相对于主体的坐标是多少？

实现分析:

  1. 坐标系相对关系，可以通过发布方发布
  2. 订阅方，订阅到发布的坐标系相对关系，再传入坐标点信息(可以写死)，然后借助于 tf 实现坐标变换，并将结果输出

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

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

  2. 编写发布方实现

  3. 编写订阅方实现

  4. 执行并查看结果

2.1 C++实现

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

2.1.1 发布方

#include"ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
/*
    需求:发布两个坐标系的相对关系

    流程:
        1.包含头文件
        2.设置编码 节点初始化 nodehandle
        3.创建发布对象
        4.组织被发布的消息
        5.发布数据
        6.spin()

*/

int main(int argc, char  *argv[])
{
    //2.设置编码 节点初始化 nodehandle
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"static_pub");
    ros::NodeHandle nh;
    //3.创建发布对象
    tf2_ros::StaticTransformBroadcaster pub;
    // 4.组织被发布的消息
    geometry_msgs::TransformStamped tfs;
    tfs.header.stamp =  ros::Time::now();
    tfs.header.frame_id = "base_link"; //相对坐标系关系中被参考的那一个
    tfs.child_frame_id = "laser";
    //相对偏移量
    tfs.transform.translation.x = 0.2;
    tfs.transform.translation.y = 0.0;
    tfs.transform.translation.z = 0.5;
    //四元数的设置需要依据欧拉角转换
    tf2::Quaternion qtn; //创建四元数对象
    //向该对象设置欧拉角,可以将欧拉角转化为四元数
    qtn.setRPY(0,0,0); //(roll pitch yaw)欧拉角的单位为弧度
    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.发布数据
     pub.sendTransform(tfs);
     // 6.spin()
    ros::spin();
    return 0;
}

运行后可以使用以下命令查看消息内容

rostopic echo /tf_static 

transforms: 
    header: 
      seq: 0
      stamp: 
        secs: 1673180975
        nsecs: 668421241
      frame_id: "base_link"
    child_frame_id: "laser"
    transform: 
      translation: 
        x: 0.2
        y: 0.0
        z: 0.5
      rotation: 
        x: 0.0
        y: 0.0
        z: 0.0
        w: 1.0

运行rivz查看坐标系相对关系,Fixed Frame设置为base_link

rosrun rviz rviz

在这里插入图片描述

2.1.2 订阅方

#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_listener.h订阅数据,buffer.h缓存(将订阅的数据存储至缓存中)
/*  
    订阅坐标系信息，生成一个相对于 子级坐标系的坐标点数据，转换成父级坐标系中的坐标点

    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建 TF 订阅节点
        4.生成一个坐标点(相对于子级坐标系)
        5.转换坐标点(相对于父级坐标系),调用tf内置功能实现
        6.spin()
*/
int main(int argc, char  *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"static_sub");
    ros::NodeHandle nh;
    // 3.创建 TF 订阅节点
    //创建一个buffer缓存
    tf2_ros::Buffer buffer;
    //再创建监听对象(监听对象可以将订阅的数据存入buffer)
    tf2_ros::TransformListener listener(buffer);
    // 4.生成一个坐标点(相对于子级坐标系)
    geometry_msgs::PointStamped ps;
    ps.header.frame_id = "laser";
    ps.header.stamp = ros::Time::now();
    ps.point.x = 2.0;
    ps.point.y = 3.0;
    ps.point.z = 5.0;
    // 解决方案1:添加休眠
    ros::Duration(2).sleep();
    //terminate called after throwing an instance of 'tf2::LookupException'
    //  what():  "base_link" passed to lookupTransform argument target_frame does not exist. 
    // 已放弃 (核心已转储)
    //原因: 发布的坐标系相对关系未拿到坐标点却开始发生转换
    // 解决方案1:添加休眠

    // 5.转换坐标点(相对于父级坐标系)
    ros::Rate rate(10);
    while (ros::ok())
    {
        //将ps转化为相对于base_link的坐标点
        geometry_msgs::PointStamped ps_out;
        ps_out = buffer.transform(ps,"base_link");
        ROS_INFO("转换后的坐标值:(x: %0.2f , y: %0.2f ,z: %0.2f),参考坐标系%s",
                                  ps_out.point.x,
                                  ps_out.point.y,
                                  ps_out.point.z,
                                  ps_out.header.frame_id.c_str()      
                                );
         // 6.spin()
        rate.sleep();
        ros::spinOnce();

    }
    
    return 0;
}


rosrun tf2_test demo01_static_pub
rosrun tf2_test demo02_static_sub

出现以下问题

terminate called after throwing an instance of 'tf2::LookupException'
  what():  "base_link" passed to lookupTransform argument target_frame does not exist. 
已放弃 (核心已转储)

原因: 发布的坐标系相对关系未拿到坐标点却开始发生转换
解决方案1:添加休眠
解决方案2:异常处理(建议)

        try
        {
            geometry_msgs::PointStamped point_base;
            point_base = buffer.transform(point_laser,"base_link");
            ROS_INFO("转换后的数据:(%.2f,%.2f,%.2f),参考的坐标系是:",point_base.point.x,point_base.point.y,point_base.point.z,point_base.header.frame_id.c_str());

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

运行成功
在这里插入图片描述PS: 调用时必须包含头文件 tf2_geometry_msgs/tf2_geometry_msgs.h,否则编译异常.

2.2 python实现

实现流程与C++基本一致

2.2.1 发布方

#! /usr/bin/env python
#-- coding:UTF-8 --
"""  
    静态坐标变换发布方:
        发布关于 laser 坐标系的位置信息 
    实现流程:
        1.导包
        2.初始化 ROS 节点
        3.创建 静态坐标广播器
        4.创建并组织被广播的消息
        5.广播器发送消息
        6.spin
"""
# 1.导包
import rospy
import tf2_ros
import tf
from geometry_msgs.msg import TransformStamped

if __name__ == "__main__":
    # 2.初始化 ROS 节点
    rospy.init_node("static_tf_pub_p")
    # 3.创建 静态坐标广播器
    broadcaster = tf2_ros.StaticTransformBroadcaster()
    # 4.创建并组织被广播的消息
    tfs = TransformStamped()
    # --- 头信息
    tfs.header.frame_id = "world"
    tfs.header.stamp = rospy.Time.now()
    tfs.header.seq = 101
    # --- 子坐标系
    tfs.child_frame_id = "radar"
    # --- 坐标系相对信息
    # ------ 偏移量
    tfs.transform.translation.x = 0.2
    tfs.transform.translation.y = 0.0
    tfs.transform.translation.z = 0.5
    # ------ 四元数
    qtn = tf.transformations.quaternion_from_euler(0,0,0)
    tfs.transform.rotation.x = qtn[0]
    tfs.transform.rotation.y = qtn[1]
    tfs.transform.rotation.z = qtn[2]
    tfs.transform.rotation.w = qtn[3]


    # 5.广播器发送消息
    broadcaster.sendTransform(tfs)
    # 6.spin
    rospy.spin()

rviz显示
在这里插入图片描述

2.2.2 订阅方

#! /usr/bin/env python
#-- coding:UTF-8 --
"""  
    订阅坐标系信息，生成一个相对于 子级坐标系的坐标点数据，
    转换成父级坐标系中的坐标点

    实现流程:
        1.导包
        2.初始化 ROS 节点
        3.创建 TF 订阅对象
        4.创建一个 radar 坐标系中的坐标点
        5.调研订阅对象的 API 将 4 中的点坐标转换成相对于 world 的坐标
        6.spin

"""
# 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 = "radar"
        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.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()

在这里插入图片描述

2.3 补充

补充1:

当坐标系之间的相对位置固定时，那么所需参数也是固定的: 父系坐标名称、子级坐标系名称、x偏移量、y偏移量、z偏移量、x 翻滚角度、y俯仰角度、z偏航角度，实现逻辑相同，参数不同，那么 ROS 系统就已经封装好了专门的节点，使用方式如下:

rosrun tf2_ros static_transform_publisher x偏移量 y偏移量 z偏移量 z偏航角度 y俯仰角度 x翻滚角度 父级坐标系 子级坐标系
示例:rosrun tf2_ros static_transform_publisher 0.2 0 0.5 0 0 0 /baselink /laser
也建议使用该种方式直接实现静态坐标系相对信息发布。

3. 动态坐标变换

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

实现分析:

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

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

  • 新建功能包，添加依赖

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

  • 创建坐标相对关系订阅方

  • 执行

3.1 C++实现

3.1.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"
/*  
    动态的坐标系相对姿态发布(一个坐标系相对于另一个坐标系的相对姿态是不断变动的)

    需求: 启动 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
*/
void doPose(const turtlesim::Pose::ConstPtr& pose)
{
    //获取位姿信息,转换并发布
    //1.创建发布对象
    static tf2_ros::TransformBroadcaster broadcaster ;
    //2.组织被发布的数据
    geometry_msgs::TransformStamped tfs;
    tfs.header.frame_id = "world";
    tfs.header.stamp = ros::Time::now();
    tfs.child_frame_id = "turtle1";
    //偏移量,从pose中获取
    /*
            位姿信息中,没有四元数,但有偏航角度.又因为乌龟是2D的,没有翻滚与俯仰角度,所以可以设置为0.
            综上,乌龟的欧拉角为0 0 theta
    */
    tfs.transform.translation.x = pose ->x;
    tfs.transform.translation.y = pose ->y;
    tfs.transform.translation.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();

    //3.发布
    broadcaster.sendTransform(tfs);
}

int main(int argc, char  *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"dynamic_pub");
    // 3.创建 ROS 句柄
    ros::NodeHandle nh;
    // 4.创建订阅对象
    ros::Subscriber sub = nh.subscribe("/turtle1/pose",100,doPose);
    // 5.回调函数处理订阅到的数据(实现TF广播)
    //      5-1.创建 TF 广播器
    //      5-2.创建 广播的数据(通过 pose 设置)
    //      5-3.广播器发布数据
    // 6.spin
    ros::spin();
    return 0;
}

rviz效果
在这里插入图片描述

3.1.2 订阅方

订阅时,静态坐标变换与动态坐标变换差别不大.

#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_listener.h订阅数据,buffer.h缓存(将订阅的数据存储至缓存中)
/*  
    订阅坐标系信息，生成一个相对于 子级坐标系的坐标点数据，转换成父级坐标系中的坐标点

    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建 TF 订阅节点
        4.生成一个坐标点(相对于子级坐标系)
        5.转换坐标点(相对于父级坐标系),调用tf内置功能实现
        6.spin()
*/
int main(int argc, char  *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"dynamic_sub");
    ros::NodeHandle nh;
    // 3.创建 TF 订阅节点
    //创建一个buffer缓存
    tf2_ros::Buffer buffer;
    //再创建监听对象(监听对象可以将订阅的数据存入buffer)
    tf2_ros::TransformListener listener(buffer);
    // 4.生成一个坐标点(相对于子级坐标系)
    geometry_msgs::PointStamped ps;
    ps.header.frame_id = "turtle1";
    ps.header.stamp = ros::Time(0,0);
    ps.point.x = 2.0;
    ps.point.y = 3.0;
    ps.point.z = 5.0;
    // 解决方案1:添加休眠
    ros::Duration(2).sleep();
    //terminate called after throwing an instance of 'tf2::LookupException'
    //  what():  "base_link" passed to lookupTransform argument target_frame does not exist. 
    // 已放弃 (核心已转储)
    //原因: 发布的坐标系相对关系未拿到坐标点却开始发生转换
    // 解决方案1:添加休眠

    // 5.转换坐标点(相对于父级坐标系)
    ros::Rate rate(10);
    while (ros::ok())
    {
        //将ps转化为相对于base_link的坐标点
        geometry_msgs::PointStamped ps_out;
        ps_out = buffer.transform(ps,"world");
        ROS_INFO("转换后的坐标值:(x: %0.2f , y: %0.2f ,z: %0.2f),参考坐标系%s",
                                  ps_out.point.x,
                                  ps_out.point.y,
                                  ps_out.point.z,
                                  ps_out.header.frame_id.c_str()      
                                );
         // 6.spin()
        rate.sleep();
        ros::spinOnce();

    }
    
    return 0;
}

日志消息

[ INFO] [1673260912.458748475]: 转换后的坐标值:(x: 6.02 , y: 10.50 ,z: 5.00),参考坐标系world
[ INFO] [1673260912.558674506]: 转换后的坐标值:(x: 6.68 , y: 10.30 ,z: 5.00),参考坐标系world
[ INFO] [1673260912.658733414]: 转换后的坐标值:(x: 7.39 , y: 9.91 ,z: 5.00),参考坐标系world
[ INFO] [1673260912.758674736]: 转换后的坐标值:(x: 7.91 , y: 9.45 ,z: 5.00),参考坐标系world
[ INFO] [1673260912.858798948]: 转换后的坐标值:(x: 8.34 , y: 8.91 ,z: 5.00),参考坐标系world
[ INFO] [1673260912.958771421]: 转换后的坐标值:(x: 8.65 , y: 8.29 ,z: 5.00),参考坐标系world
[ INFO] [1673260913.058666995]: 转换后的坐标值:(x: 8.86 , y: 7.52 ,z: 5.00),参考坐标系world
[ INFO] [1673260913.158740752]: 转换后的坐标值:(x: 8.97 , y: 7.19 ,z: 5.00),参考坐标系world
[ INFO] [1673260913.258723715]: 转换后的坐标值:(x: 9.09 , y: 7.04 ,z: 5.00),参考坐标系world
[ INFO] [1673260914.658674377]: 转换后的坐标值:(x: 10.84 , y: 4.84 ,z: 5.00),参考坐标系world
[ INFO] [1673260914.758673053]: 转换后的坐标值:(x: 10.93 , y: 4.83 ,z: 5.00),参考坐标系world
[ INFO] [1673260914.858675805]: 转换后的坐标值:(x: 10.79 , y: 5.50 ,z: 5.00),参考坐标系world
[ INFO] [1673260914.958716544]: 转换后的坐标值:(x: 10.51 , y: 6.14 ,z: 5.00),参考坐标系world
[ INFO] [1673260915.058708439]: 转换后的坐标值:(x: 10.05 , y: 6.80 ,z: 5.00),参考坐标系world
[ INFO] [1673260915.158602451]: 转换后的坐标值:(x: 9.71 , y: 7.06 ,z: 5.00),参考坐标系world
[ INFO] [1673260915.258678036]: 转换后的坐标值:(x: 9.52 , y: 7.09 ,z: 5.00),参考坐标系world

注意
ps.header.stamp = ros::Time(0,0);不能用原来的 tfs.header.stamp = ros::Time::now();.

原因:使用 ros::Time::now();时因为在动态坐标变换中,buffer缓存中会有许多值,这些值是由发布方不停发布的,且每次发布时,具有不同的时间戳.在进行坐标转换时,会将坐标点的时间戳和坐标关系的时间戳进行比对,若两者之间差距较小,则没什么大问题;若差距较大,则会抛异常.

3.2 python实现

3.2.1 发布方

#! /usr/bin/env python
#-- coding:UTF-8 --
"""  
    动态的坐标系相对姿态发布(一个坐标系相对于另一个坐标系的相对姿态是不断变动的)

    需求: 启动 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.2.2 订阅方

#! /usr/bin/env python
#-- coding:UTF-8 --
"""  
    动态的坐标系相对姿态发布(一个坐标系相对于另一个坐标系的相对姿态是不断变动的)

    需求: 启动 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()

在这里插入图片描述

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

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

相关文章

Gin框架快速入门实战

Gin框架快速入门实战

gin 框架快速入门 工具 测试工具&#xff1a; 浏览器拓展 postwomanapipost gin路由&#xff0c;gin 程序的热加载 gin官网文档&#xff1a; https://gin-gonic.com/zh-cn/docs/ fresh安装 &#xff1a; go get github.com/pilu/fresh package mainimport ("github…
阅读更多...
Revit标注时尺寸界线的设置及标注避让调整功能

Revit标注时尺寸界线的设置及标注避让调整功能

一、Revit标注时尺寸界线的设置 利用墙体一次性标注轴网尺寸时常遇到如图1所示尺寸边界线上翻的情况&#xff0c;需要手动将其逐一调整&#xff0c;十分繁琐&#xff0c;有没有更为快速地解决方法呢&#xff1f; 此问题可以通过为这些尺寸界线反向显示的尺寸标注单独设置一个新…
阅读更多...
CHK文件丢失怎么办？chk文件恢复技巧就看这一个！

CHK文件丢失怎么办？chk文件恢复技巧就看这一个！

很多人不知道CHK文件是什么&#xff0c;其实它是一种后缀名为CHK格式的文件&#xff0c;也属于日常生活中比较常见的文件格式。通常&#xff0c;当用户使用“磁盘碎片整理”时&#xff0c;电脑系统会生成一串“群集”文件&#xff0c;这些文件主要用于保存计算机删除的文件的一…
阅读更多...
【中等】最长回文子串-C语言实现

【中等】最长回文子串-C语言实现

题目链接&#xff1a;https://leetcode.cn/problems/longest-palindromic-substring/思路&#xff1a;起始位置为初始位置向右走&#xff0c;即第一次a为起始位置&#xff0c;第二次b为起始位置结束位置从末尾向左走&#xff0c;即第一次c为末尾&#xff0c;第二次b为末尾&…
阅读更多...
BMS中常用的NTC温敏电阻及代码实现

BMS中常用的NTC温敏电阻及代码实现

1、什么是NTC&#xff1f; NTC热敏电阻是一种负温度系数的热敏电阻&#xff0c;它的性性是阻值随温度的升高而降低&#xff0c;主要作用是对温度的测量及补偿&#xff0c;也用于NTC温度传感器的制作&#xff0c;常用的使用范围在-55℃至200℃之间。 2、NTC的主要技术参数有哪些…
阅读更多...
重发布-路由策略实验1（1.8）

重发布-路由策略实验1（1.8）

目标&#xff1a; 1、首先为每个路由器配置环回和每个接口的ip r1&#xff1a; [r1]interface lo0 [r1-LoopBack0]ip add 1.1.1.1 24 [r1-LoopBack0]int gi 0/0/0 [r1-GigabitEthernet0/0/0]ip add 12.1.1.1 24 [r1-GigabitEthernet0/0/0]int gi 0/0/1 [r1-GigabitEthernet0/…
阅读更多...
Linux--多线程（2）

Linux--多线程（2）

目录1. 条件变量2. 生产者消费者模型2.1 概念3. 基于BlockingQueue的生产者消费者模型3.1 概念3.2 等待函数3.3 等待函数的功能3.4 唤醒函数4. 模型复盘5. 总代码1. 条件变量 当一个线程互斥地访问某个变量或者临界资源时&#xff0c;它可能发现在其它线程改变状态之前&#x…
阅读更多...
物以类聚人以群分,通过GensimLda文本聚类构建人工智能个性化推荐系统(Python3.10)

物以类聚人以群分,通过GensimLda文本聚类构建人工智能个性化推荐系统(Python3.10)

众所周知&#xff0c;个性化推荐系统能够根据用户的兴趣、偏好等信息向用户推荐相关内容&#xff0c;使得用户更感兴趣&#xff0c;从而提升用户体验&#xff0c;提高用户粘度&#xff0c;之前我们曾经使用协同过滤算法构建过个性化推荐系统&#xff0c;但基于显式反馈的算法就…
阅读更多...
【Java寒假打卡】Java基础-集合Map

【Java寒假打卡】Java基础-集合Map

【Java寒假打卡】Java基础-集合Map基本使用Map集合的基本功能Map集合的第一种遍历方式Map集合的第二种遍历方式案例&#xff1a;HashMap集合存储ArrayList元素并遍历案例&#xff1a;统计字符串中每一个字符出现的次数Collections操纵集合基本使用 创建Map集合的对象&#xff1…
阅读更多...
金融历史数据导入之股票 level2 逐笔篇

金融历史数据导入之股票 level2 逐笔篇

在部署完 DolphinDB 后&#xff0c;将历史数据导入数据库是后续进行数据查询、计算和分析的基础。为协助用户快速导入数据&#xff0c;本文档基于 DolphinDB 已有的教程与大量用户的实践经验&#xff0c;从操作者角度出发&#xff0c;以 CSV 格式的文件为例&#xff0c;详细介绍…
阅读更多...
通讯电平转换电路中的经典设计

通讯电平转换电路中的经典设计

今天给大家分享几个通讯电平转换电路。 有初学者问&#xff1a;什么是电平转换&#xff1f;举个例子&#xff0c;比如下面这个电路&#xff1a; 单片机的工作电压是5V&#xff0c;蓝牙模块的工作电压是3.3V&#xff0c;两者之间要进行通讯&#xff0c;TXD和RXD引脚就要进行连接…
阅读更多...
Revit里轴网隐藏尺寸标注跟着消失？快速轴网距离标注

Revit里轴网隐藏尺寸标注跟着消失？快速轴网距离标注

一、Revit中链接项目文件轴网的巧妙处理 问题&#xff1a;在单元式住宅体系中&#xff0c;轴网的使用主要是对尺寸标注的影响&#xff0c;如果要将子文件链接到父文件中&#xff0c;需要隐藏轴网&#xff0c;这样与轴网关联的尺寸标注就会消失。 关于尺寸标注与轴网隐藏方式的关…
阅读更多...
Java IO流（基础详解，快速上手！）

Java IO流（基础详解，快速上手！）

文章目录概述什么是IO流&#xff1f;常用的文件操作获取文件操作目录操作IO流的原理和分类概述 在Java的学习中&#xff0c;文件和IO流是一个十分重要的板块。在Java中&#xff0c;File是文件和目录路径名的抽象表示。文件和目录可以通过File封装成对象。对File而言&#xff0…
阅读更多...
前端 base64与图片相互转换

前端 base64与图片相互转换

base64转图片 如下图&#xff1a;&#xff08;后端返回的数据&#xff09; <img :src"baseImg" >let baseImg "" this.baseImg "data:image/png;base64," data?.flowCharbase64转换图片文件 base64ImgtoFile (dataurl, filename …
阅读更多...
QT 学习笔记（十六）

QT 学习笔记（十六）

文章目录一、TCP 传文件流程图1. 服务器端流程2. 客户端流程二、TCP 传文件操作实现1. 服务器端2. 客户端3. TCP 传文件实现现象三、服务器端和客户端实现代码1. 主函数 main.c2. 服务器端头文件 serverwidget.h3. 服务器端源文件 serverwidget.cpp4. 客户端头文件 clientwidge…
阅读更多...
某医院的实战渗透测试（组合拳）

某医院的实战渗透测试（组合拳）

实战渗透一、前言二、Spring信息泄露三、Redis写公钥四、文章来源一、前言 项目是内网环境下进行&#xff0c;所以通过vpn接入内网之后进行目标系统的测试。&#xff08;信息泄露redis写公钥&#xff09; 二、Spring信息泄露 访问客户给的目标地址通过代理把流量转给了BurpS…
阅读更多...
零基础学软件测试有前途吗？

零基础学软件测试有前途吗？

随着软件工程活动的不断演化&#xff0c;测试工作某种程度上是可以很大幅度提高软件的产品质量以及提升用户的使用满意度&#xff0c;因此软件测试工程师的地位在企业中也越来越受到重视。不少零基础学IT的朋友也开始把软件测试作为一个绝佳的选择对象&#xff0c;那么零基础学…
阅读更多...
leetcode.1806 还原排列的最少操作步数 - 模拟 + lcm

leetcode.1806 还原排列的最少操作步数 - 模拟 + lcm

​​​​​​1806. 还原排列的最少操作步数 本题是数论题 共介绍4种解题方法 目录 1、所有置换环长度的最小公倍数 2、最小操作数是最大环长度 3、1或n-2所在环长度即为最大置换环长度 4、暴力模拟 思路&#xff1a; 因为数据范围很小 所以可以直接模拟 也可以优化一下—…
阅读更多...
Python 模型训练：LSTM 时间序列销售额预测（训练、保存、调用）

Python 模型训练：LSTM 时间序列销售额预测（训练、保存、调用）

LSTM (long short-term memory) 长短期记忆网络&#xff0c;具体理论的就不一一叙述&#xff0c;直接开始 流程一、数据导入二、数据归一化三、划分训练集、测试集四、划分标签和属性五、转换成 LSTM 输入格式六、设计 LSTM 模型6.1 直接建模6.2 找最好七、测试与图形化展示八、…
阅读更多...
JavaSE-07

JavaSE-07

字节流输入输出数据&#xff1a; InputStream和OutputStream作为字节流输入输出流的超类。 字节流写数据时千万记得close关闭资源&#xff0c;可设置追加写为true 字节流读数据时&#xff0c;FileInputStream a new FileInputStream (“”); int by a.read(); char b (char…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023