上次我们学习了 TF 的基本概念和如何发布静态的 TF 坐标:

这次来总结下如何发布一个自定义的 TF 坐标转换,并监听这个变换。

一、编写 TF 广播者

进入上次创建的 learning_tf2 包中:

roscd learning_tf2

src 下新建一个 turtle_tf2_broadcaster.cpp 文件,代码如下:

#include <ros/ros.h>

// 存储要发布的坐标变换
#include <geometry_msgs/TransformStamped.h> // 四元数
#include <tf2/LinearMath/Quaternion.h> // 变换广播者
#include <tf2_ros/transform_broadcaster.h> // 乌龟的位姿定义
#include <turtlesim/Pose.h> std::string turtle_name; void poseCallback(const turtlesim::PoseConstPtr& msg)
{
// 创建 tf 广播对象
static tf2_ros::TransformBroadcaster br; // 存储要发布的坐标变换消息
geometry_msgs::TransformStamped transformStamped; // 变换的时间戳
transformStamped.header.stamp = ros::Time::now(); // 父坐标系名称
transformStamped.header.frame_id = "world"; // 当前要发布的坐标系名称 - 乌龟的名字
transformStamped.child_frame_id = turtle_name; // 乌龟在二维平面运动,所以 z 坐标高度为 0
transformStamped.transform.translation.x = msg->x;
transformStamped.transform.translation.y = msg->y;
transformStamped.transform.translation.z = 0.0; // 用四元数存储乌龟的旋转角
tf2::Quaternion q; // 因为乌龟在二维平面运动,只能绕 z 轴旋转,所以 x,y 轴的旋转量为 0
q.setRPY(0, 0, msg->theta); // 把四元数拷贝到要发布的坐标变换中
transformStamped.transform.rotation.x = q.x();
transformStamped.transform.rotation.y = q.y();
transformStamped.transform.rotation.z = q.z();
transformStamped.transform.rotation.w = q.w(); // 用 tf 广播者把订阅的乌龟位姿发布到 tf 中
br.sendTransform(transformStamped);
} int main(int argc, char** argv)
{
// 当前节点的名称
ros::init(argc, argv, "my_tf2_broadcaster");
ros::NodeHandle private_node("~"); // 判断当前要广播的乌龟节点名字
if (!private_node.hasParam("turtle")) {
// launch 文件和命令行都没有传递乌龟名称,就直接退出
if (argc != 2) {
ROS_ERROR("need turtle name as argument");
return -1;
}; // launch 文件中如果没有定义乌龟名称,就在命令行中加上
turtle_name = argv[1];
} else {
// 从 launch 文件获取乌龟名称参数
private_node.getParam("turtle", turtle_name);
} ros::NodeHandle node; // 订阅一个节点的 pose msg,在回调函数中广播订阅的位姿消息到 tf2 坐标系统中
// turtle_name 为 turtle1 时广播 turtle1 的位姿到 tf 中
// turtle_name 为 turtle2 时广播 turtle2 的位姿到 tf 中
ros::Subscriber sub = node.subscribe(turtle_name + "/pose", 10, &poseCallback); ros::spin();
return 0;
};

这个程序的意思是订阅输入乌龟的 pose 话题,然后在 poseCallback 回调函数中发布 world 到乌龟的 TF 变换,注意这个程序可以接收不同乌龟的 pose 消息,只要运行时指定乌龟的名称 turtle_name 即可,代码注释很详细,其他的就不说了,然后添加编译规则:

add_executable(turtle_tf2_broadcaster src/turtle_tf2_broadcaster.cpp)
target_link_libraries(turtle_tf2_broadcaster ${catkin_LIBRARIES})

直接编译:

catkin_make

基本上不会出问题,为了方便启动我们在 launch 文件中启动广播者:

<launch>
<!-- 乌龟节点 -->
<node pkg="turtlesim" type="turtlesim_node" name="sim"/> <!-- 控制乌龟运动的键盘节点 -->
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/> <!-- 线速度和角速度的定义,但是在这个例子中并没有用到哎... -->
<param name="scale_linear" value="2" type="double"/>
<param name="scale_angular" value="2" type="double"/> <!-- 第一个乌龟的 tf 广播者节点,参数为乌龟 1 的名字 /tutle1 -->
<node pkg="learning_tf2" type="turtle_tf2_broadcaster" args="/turtle1" name="turtle1_tf2_broadcaster" /> <!-- 第二个乌龟的 tf 广播者节点,还是用相同的节点,只不过改变了传递的参数为乌龟 2 的名字 /turtle2 -->
<node pkg="learning_tf2" type="turtle_tf2_broadcaster" args="/turtle2" name="turtle2_tf2_broadcaster" /> </launch>

然后就可以直接启动了:

roslaunch learning_tf2 start_demo.launch

为了确定是否成功广播了变换,使用下面的命令查看一个变换的输出:

rosrun tf tf_echo /world /turtle1

如果在控制台输出类似下面的消息,则说明变换发布成功:

下面我们来编写一个 TF 接收者来使用我们上面发布的变换。

二、编写 TF 接收者

同样在 src 目录下创建 turtle_tf2_listener.cpp,代码如下:

#include <ros/ros.h>

// 接受 tf 变换
#include <tf2_ros/transform_listener.h> // 转换消息
#include <geometry_msgs/TransformStamped.h> // 发布到乌龟 2 的运动消息:角速度和线速度
#include <geometry_msgs/Twist.h> // 再生服务
#include <turtlesim/Spawn.h> // 实现乌龟 2 跟随乌龟 1 运动
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); // tf 变换缓存区,最多缓存 10 秒
tf2_ros::Buffer tfBuffer; // 创建监听 tf 变换对象,创建完毕即开始监听,通常定义为成员变量
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; // 新的角速度为寻找到的变换角速度的 4 倍 - 使得第二个乌龟的运动轨迹转弯更快,且轨迹是弧线
vel_msg.angular.z = 4.0 * atan2(transformStamped.transform.translation.y, transformStamped.transform.translation.x); // 新的线速度是寻找到的变换线速度的 0.5 倍 - 使得第二个乌龟的运动速度为第一个乌龟的一半
vel_msg.linear.x = 0.5 * sqrt(pow(transformStamped.transform.translation.x, 2) + pow(transformStamped.transform.translation.y, 2)); // 发布新的速度消息,乌龟 2 节点的内部订阅了这个消息,所以乌龟 2 会收到新的角速度和线速度,以此产生跟随运动
turtle_vel.publish(vel_msg); rate.sleep();
} return 0;
};

这里关键的代码如下:

// 保存寻找的变换
geometry_msgs::TransformStamped transformStamped; // 寻找 turtle1 到 turtle2 的坐标变换
// target_frame: turtle2
// source_frame: turtle1
// ros::Time(0): 获取变换的时间,
transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time(0));

同样添加编译规则:

add_executable(turtle_tf2_listener src/turtle_tf2_listener.cpp)
target_link_libraries(turtle_tf2_listener ${catkin_LIBRARIES})

然后编译:

catkin_make

在上面广播者的 launch 文件中加上接收者的启动:

<!--
这个例子一共创建了 5 个节点:
1. 乌龟节点,包含 2 个小乌龟
2. 控制乌龟运动的键盘节点
3. 第一个乌龟的 tf 广播者节点
4. 第二个乌龟的 tf 广播者节点
5. tf 坐标系统的监听节点,用来监听 2 个乌龟之间的坐标变换
-->
<launch>
<!-- 乌龟节点,这个节点的内部应该是创建了 2 个乌龟...... -->
<node pkg="turtlesim" type="turtlesim_node" name="sim"/> <!-- 控制乌龟运动的键盘节点 -->
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/> <!-- 线速度和角速度的定义,但是在这个例子中并没有用到哎... -->
<param name="scale_linear" value="2" type="double"/>
<param name="scale_angular" value="2" type="double"/> <!-- 第一个乌龟的 tf 广播者节点,参数为乌龟 1 的名字 /tutle1 -->
<node pkg="learning_tf2" type="turtle_tf2_broadcaster" args="/turtle1" name="turtle1_tf2_broadcaster" /> <!-- 第二个乌龟的 tf 广播者节点,还是用相同的节点,只不过改变了传递的参数为乌龟 2 的名字 /turtle2 -->
<node pkg="learning_tf2" type="turtle_tf2_broadcaster" args="/turtle2" name="turtle2_tf2_broadcaster" /> <!-- 启动 tf 坐标系同的监听节点 -->
<node pkg="learning_tf2" type="turtle_tf2_listener" name="listener" /> </launch>

然后启动:

roslaunch learning_tf2 start_demo.launch

运行时会出现 2 个小乌龟,把窗口焦点放到终端,按上下左右键会发现第二个乌龟跟随第一个乌龟运动:

但是刚启动时终端会报个错误:

[ERROR] [1418082761.220546623]: "turtle2" passed to lookupTransform argument target_frame does not exist.
[ERROR] [1418082761.320422000]: "turtle2" passed to lookupTransform argument target_frame does not exist.

这是因为我们在 turtle2 还没有产生之前就寻找变换,导致没有找到它,为了解决这个问题可以在寻找变换前等待变换可用:

// 第四个参数是阻塞等待的超时时间
listener.waitForTransform("/turtle2", "/turtle1", ros::Time::now(), ros::Duration(3.0)); transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time(0));

加上这句运行时就不会报错了,今天就写到这里,下次见:)

ROS 机器人技术 - 广播与接收 TF 坐标的更多相关文章

  1. ROS 机器人技术 - 解决 ROS_INFO 不能正确输出 string 的问题!

    一.输出「??」 项目调试一个节点,打印 ROS 信息时发现设置的节点名称都是问号: ROS_INFO("[%s]: camera_extrinsic_mat", kNodeNam ...

  2. ros机器人开发概述

    1.       ROS项目开发流程? 参照古月大神写的ROS探索总结系列:http://blog.exbot.net/archives/619 具体项目设计可看看<程序员>杂志的最新一篇 ...

  3. ROS机器人程序设计(原书第2版)补充资料 (柒) 第七章 3D建模与仿真 urdf Gazebo V-Rep Webots Morse

    ROS机器人程序设计(原书第2版)补充资料 (柒) 第七章 3D建模与仿真 urdf Gazebo V-Rep Webots Morse 书中,大部分出现hydro的地方,直接替换为indigo或ja ...

  4. ROS机器人操作系统相关书籍、资料和学习路径

    作者:Top Liu链接:https://zhuanlan.zhihu.com/p/30391098来源:知乎著作权归作者所有.商业转载请联系作者获得授权,非商业转载请注明出处. 本文是易科机器人实验 ...

  5. ROS机器人程序设计(原书第2版)补充资料 教学大纲

    ROS机器人程序设计(原书第2版) 补充资料 教学大纲 针对该书稍后会补充教学大纲.教案.多媒体课件以及练习题等. <ROS机器人程序设计>课程简介 课程编号:XXXXXX 课程名称:RO ...

  6. ROS机器人程序设计-学习小结-

    ROS官网 |易科 |虞坤林 |古月居 |ROSClub 学习ROS相关书籍推荐:http://blog.csdn.net/zhangrelay/article/details/52244746 RO ...

  7. ROS机器人程序设计(原书第2版)补充资料 (拾) 第十章 使用MoveIt!

    ROS机器人程序设计(原书第2版)补充资料 (拾) 第十章 使用MoveIt! 书中,大部分出现hydro的地方,直接替换为indigo或jade或kinetic,即可在对应版本中使用. MoveIt ...

  8. ROS机器人程序设计(原书第2版)补充资料 (捌) 第八章 导航功能包集入门 navigation

    ROS机器人程序设计(原书第2版)补充资料 (捌) 第八章 导航功能包集入门 navigation 书中,大部分出现hydro的地方,直接替换为indigo或jade或kinetic,即可在对应版本中 ...

  9. ROS机器人程序设计(原书第2版)补充资料 (壹) 第一章 ROS系统入门

    ROS机器人程序设计(原书第2版)补充资料 (壹) 第一章 ROS系统入门 书中,大部分出现hydro的地方,直接替换为indigo或jade或kinetic,即可在对应版本中使用. 第一章主要包括R ...

随机推荐

  1. CSS背景颜色透明

    { filter:alpha(opacity=50); -moz-opacity:0.5; -khtml-opacity: 0.5; opacity: 0.5; } 兼容大部分主流浏览器 filter ...

  2. Fiddler和JMeter测试需要主要的地方

    Fiddler里面设置请求头的时候ContentType和Content-Type这两种写法都可以: 这两种写法都可以. 但是在JMeter中必须要用Content-Type才行,如下图所示: (完)

  3. CSS五种方式实现 Footer 置底

    页脚置底(Sticky footer)就是让网页的footer部分始终在浏览器窗口的底部.当网页内容足够长以至超出浏览器可视高度时,页脚会随着内容被推到网页底部:但如果网页内容不够长,置底的页脚就会保 ...

  4. 请解释ASP. NET中的web页面与隐藏类之间的关系

    请解释ASP.NET中的web页面与其隐藏类之间的关系 其实页面与其隐藏类之间就是一个部分类的关系,你在页面上放一个一个的控件就是在这个类中定义一个一个的属性, 因为是同一个类的部分类的关系,所以隐藏 ...

  5. 「疫期集训day0」启程

    看了看几乎所有学长都是写的博客,所以写的博客 由于是第一回集训,考得都是老题(虽然有些还不会) 感受1:我调试好蒻呃,调试巨蒻,T1lis模板5分切,结果T2T3T4调了将近了两个小时,先是T2路径输 ...

  6. unity vscode 断点问题

    困扰了很久的vscode老莫名其妙的断到网络通信那里. 后来发现是因为起来了一个线程并且调用的unity API 导致. unity 线程中是禁止调用unity API 的. 删掉用 DateTime ...

  7. docker安装,卸载和入门

    Docker 简介 背景 开发和运维之间因为环境不同而导致的矛盾 集群环境下每台机器部署相同的应用 DevOps(Development and Operations) 简介 Docker是一个开源的 ...

  8. P2295 MICE 网格中的DP

    题目描述 分析 很好的一道网格中的\(DP\)题 我们设\(f[x][y]\)为小象到达坐标为\((x,y)\)的点时看到的最少的老鼠的数量 但是这样定义是不好转移的,因为小象可能从上面的格子转移下来 ...

  9. day68 form组件

    目录 一.自定义分页器的拷贝和使用 二.Forms组件 1 前戏 2 form组件的基本功能 3 基本使用 4 基本方法 5 渲染标签 6 展示提示信息 7 钩子函数(HOOK) 8 forms组件其 ...

  10. python 并发专题(十二):基础部分补充(四)协程

    相关概念: 协程:一个线程并发的处理任务 串行:一个线程执行一个任务,执行完毕之后,执行下一个任务 并行:多个CPU执行多个任务,4个CPU执行4个任务 并发:一个CPU执行多个任务,看起来像是同时执 ...