TF2 坐标变换 监听 实例

本文涉及的产品
资源编排,不限时长
简介: TF2 坐标变换 监听 实例

这片博客 教你 如何使用TF2 实现 监听 获得 一个 坐标变换

在上一篇博客 创建了一个TF2 的 广播者 ,来发布一个 乌龟的位置 到 TF2, 这个博客 将 创建 一个 TF2 的 监听者 来开始使用TF2.

1、如何创建一个TF2的监听者

1.1 代码

在learning_tf2 的功能包的src 文件夹下 创建一个新的cpp文件 命名为 turtle_tf2_listener.cpp.

整体代码如下:

#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf2_listener");

  ros::NodeHandle node;

  ros::service::waitForService("spawn");
  ros::ServiceClient spawner =
    node.serviceClient<turtlesim::Spawn>("spawn");
  turtlesim::Spawn turtle;
  turtle.request.x = 4;
  turtle.request.y = 2;
  turtle.request.theta = 0;
  turtle.request.name = "turtle2";
  spawner.call(turtle);

  ros::Publisher turtle_vel =
    node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);

  tf2_ros::Buffer tfBuffer;
  tf2_ros::TransformListener tfListener(tfBuffer);

  ros::Rate rate(10.0);
  while (node.ok()){
    geometry_msgs::TransformStamped transformStamped;
    try{
      transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1",
                               ros::Time(0));
    }
    catch (tf2::TransformException &ex) {
      ROS_WARN("%s",ex.what());
      ros::Duration(1.0).sleep();
      continue;
    }

    geometry_msgs::Twist vel_msg;

    vel_msg.angular.z = 4.0 * atan2(transformStamped.transform.translation.y,
                                    transformStamped.transform.translation.x);
    vel_msg.linear.x = 0.5 * sqrt(pow(transformStamped.transform.translation.x, 2) +
                                  pow(transformStamped.transform.translation.y, 2));
    turtle_vel.publish(vel_msg);

    rate.sleep();
  }
  return 0;
};
AI 代码解读

1.2 代码解释

=====================================================================

#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>
AI 代码解读

以上是需要包含的文件

tf2软件包提供了TransformListener的实现,以帮助简化接收转换的任务。 要使用TransformListener,需要包含tf2 / transform_listener.h头文件。

=====================================================================

  ros::service::waitForService("spawn");
  ros::ServiceClient spawner =
    node.serviceClient<turtlesim::Spawn>("spawn");
  turtlesim::Spawn turtle;
  turtle.request.x = 4;
  turtle.request.y = 2;
  turtle.request.theta = 0;
  turtle.request.name = "turtle2";
  spawner.call(turtle);
AI 代码解读

turtle 相关 如此 则 创建 了 一个 乌龟 命名为:turtle2 初始位置 在 x=4,y=2 角度 为0

=====================================================================

  ros::Publisher turtle_vel =
    node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);
AI 代码解读

声明 速度 topic 发布 句柄 下面 讲 发布 速度控制的 消息 来控制 turtle2 运动

=====================================================================

重点

  tf2_ros::Buffer tfBuffer;
  tf2_ros::TransformListener tfListener(tfBuffer);
AI 代码解读

在这里,创建一个TransformListener对象。 创建侦听器后,它将开始通过网络接收tf2转换,并将其最多缓冲10秒钟。 TransformListener对象的作用域应为持久性,否则其缓存将无法填充,并且几乎每个查询都会失败。 一种常见的方法是使TransformListener对象成为类的成员变量。

tf2_ros::TransformListener tfListener(tfBuffer)

这样 tfListener 相当于 TransformListener 类的实例 ,tfBuffer 相当于其成员变量

=====================================================================

    geometry_msgs::TransformStamped transformStamped;
AI 代码解读

声明 TF2 的标准 消息类型 变量 ,用于存放 监听的TF2

=====================================================================

重点

    try{
      transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1",
                               ros::Time(0));
    }
    catch (tf2::TransformException &ex) {
      ROS_WARN("%s",ex.what());
      ros::Duration(1.0).sleep();
      continue;
    }
AI 代码解读

这里是真正工作的地方,查询监听者一个指定的坐标变换。
通过 lookupTransform () 函数 实现
这个函数有四个参数
lookupTransform 的 使用 说明

  1. 希望 变换到的 坐标系
  2. 从这个坐标系 (参数2),转换到目标坐标系(参数1)
  3. 转化希望时间 , ros::Time(0) 即为 得到最新的 可用坐标变换、
  4. 超时前的时间 , 可选 默认为 default=ros::Duration(0.0)

lookupTransform("turtle2", "turtle1",ros::Time(0)
这样即获得了 有 turtle1(坐标系) 到 turtle2 (坐标系)的 坐标变换

=====================================================================

    geometry_msgs::Twist vel_msg;

    vel_msg.angular.z = 4.0 * atan2(transformStamped.transform.translation.y,
                                    transformStamped.transform.translation.x);
    vel_msg.linear.x = 0.5 * sqrt(pow(transformStamped.transform.translation.x, 2) +
                                  pow(transformStamped.transform.translation.y, 2));
    turtle_vel.publish(vel_msg);
AI 代码解读

乌龟1 的位置 在 turtle1(坐标系) 的 原点
所以 transformStamped.transform.translation.x / y 就是 以 乌龟2 为 原点在 turtle2 (坐标系)下的 位置。

通过这个位置转化为 速度 控制量 来控制 乌龟2 的 运动 实现 乌龟2 跟着 乌龟 1 的效果

=====================================================================

2、运行这个监听者

在 CMakeLists.txt 中 加入 如下 代码:

add_executable(turtle_tf2_listener src/turtle_tf2_listener.cpp)
target_link_libraries(turtle_tf2_listener
 ${catkin_LIBRARIES}
)
AI 代码解读

编译成功的话

会 在工作空间 的这个路径下面 生成 turtle_tf2_listener 的 bin 文件
在这里插入图片描述

下面添加 launch 文件 给这个demo

  <launch>
     <!-- Turtlesim Node-->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>

    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
    <!-- Axes -->
    <param name="scale_linear" value="2" type="double"/>
    <param name="scale_angular" value="2" type="double"/>

    <node pkg="learning_tf2" type="turtle_tf2_broadcaster"
          args="/turtle1" name="turtle1_tf2_broadcaster" />


    <node pkg="learning_tf2" type="turtle_tf2_broadcaster"
          args="/turtle2" name="turtle2_tf2_broadcaster" />


    <node pkg="learning_tf2" type="turtle_tf2_listener"
          name="listener" />

  </launch>
AI 代码解读

运行

 $ roslaunch learning_tf2 start_demo.launch
AI 代码解读

会看到 有2 只 乌龟 在屏幕上 , 乌龟1 不动 ,乌龟2 和乌龟1 初始位置不一样 ,一会 乌龟2 就到了 乌龟1 的地方

移动 乌龟 1

rosrun turtlesim turtle_teleop_key
AI 代码解读

乌龟2 会 跟着 乌龟 1 移动 最后 两者 位置 重合

在这里插入图片描述

3、完毕

相关实践学习
使用ROS创建VPC和VSwitch
本场景主要介绍如何利用阿里云资源编排服务,定义资源编排模板,实现自动化创建阿里云专有网络和交换机。
阿里云资源编排ROS使用教程
资源编排(Resource Orchestration)是一种简单易用的云计算资源管理和自动化运维服务。用户通过模板描述多个云计算资源的依赖关系、配置等,并自动完成所有资源的创建和配置,以达到自动化部署、运维等目的。编排模板同时也是一种标准化的资源和应用交付方式,并且可以随时编辑修改,使基础设施即代码(Infrastructure as Code)成为可能。 产品详情:https://www.aliyun.com/product/ros/
相关文章
创建变量tensor
【8月更文挑战第13天】 创建变量tensor。
40 1
|
7月前
创建变量tensor
【8月更文挑战第12天】创建变量tensor。
37 1
|
10月前
|
函数形状的定义方式
函数形状的定义方式
106 0
【ROS】TF2坐标转换及实战示例
ROS中提供了坐标转换的软件包 Transform Frame TF的作用是ROS中实现不同坐标点/向量的转换。
794 0
A网络的embedding层的权重参数已经初始化为F了,copy.deepcopy(A)的结果网络也跟着初始化为F了嘛?
A网络的embedding层的权重参数已经通过 self.embedding.weight.data.copy_(pretrained_embeddings)初始化为F,那么 copy.deepcopy(A)的结果网络也跟着初始化为F了嘛?
223 0
ROS TF2当前坐标系如何计算其它历史坐标系的坐标变换
ROS TF2当前坐标系如何计算其它历史坐标系的坐标变换
ROS TF2当前坐标系如何计算其它历史坐标系的坐标变换
【古月21讲】ROS入门系列(4)——参数使用与编程方法、坐标管理系统、tf坐标系广播与监听的编程实现、launch启动文件的使用方法
【古月21讲】ROS入门系列(4)——参数使用与编程方法、坐标管理系统、tf坐标系广播与监听的编程实现、launch启动文件的使用方法
296 0
【古月21讲】ROS入门系列(4)——参数使用与编程方法、坐标管理系统、tf坐标系广播与监听的编程实现、launch启动文件的使用方法