（五）ROS 发布tf消息并在rviz中显示

1.总体步骤介绍及实验结果

本文通过实现ROS中的示例来理解ROS中的坐标变换tf

参考的英文教程如下

http://wiki.ros.org/tf/Tutorials/Introduction%20to%20tf

以上的ROS教程有两只乌龟，比较麻烦。

本文将修改教程的程序，将两只乌龟变为一只乌龟，使理解TF的使用更加容易。

本程序实现的功能是使用键盘控制乌龟在TurtleSim中移动，然后通过turtlesim节点发送广播消息，最后在rviz中显示变换。



节点关系图如下



如图，可以分为几个步骤

（1）键盘节点teleop发送/turtle1/cmd_vel消息给 turtlesim节点，turtlesim节点解析命令并控制乌龟移动。

（2）turtlesim节点发送/turtle1/pose消息给my_turtle1_tf_broadcaster节点。

（3）my_turtle1_tf_broadcaster节点发送/tf消息。

（4）最后rviz解析/tf消息并显示，从而实现坐标变换可视化。

节点就是一个单一的进程，其中键盘控制节点和sim节点通过启动相应的包的节点来实现。本文主要阐述turtle1_tf_broadcaster节点的c++实现及通过rviz显示坐标变换的.rviz系统文件的配置。

2.示例的具体实现

1.启动teleop节点

rosrun turtlesim turtle_teleop_key

2.启动sim节点

rosrun turtlesim turtlesim_node

3.实现turtle1_tf_broadcaster，并启动learning_tf包下的turtle_tf_broadcaster节点

具体实现过程如

http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20broadcaster%20%28C%2B%2B%29

主要是编辑turtle_tf_broadcaster.cpp代码并生成learning_tf包，注意节点的可执行文件名为turtle_tf_broadcaster

turtle_tf_broadcaster.cpp代码如下

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr& msg){
static tf::TransformBroadcaster br;
tf::Transform transform;
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
tf::Quaternion q;
q.setRPY(0, 0, msg->theta);
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

int main(int argc, char** argv){
ros::init(argc, argv, "my_tf_broadcaster");
if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};
turtle_name = argv[1];

ros::NodeHandle node;
ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

ros::spin();
return 0;
};

编译好turtle_tf_broadcaster后输入

rosrun learning_tf turtle_tf_broadcaster /turtle1

启动turtle_tf_broadcaster节点。

rqt_graph可以看到节点图如下

4.启动rqt_rviz工具显示坐标变换

rosrun rqt_rviz rqt_rviz

并加载配置文件,点击rqt_rviz的file菜单，open config 找到exampe_tf.rviz文件并打开。

用键盘控制，可以发现坐标在移动，实现了使用rviz显示坐标变换。

其中example_tf.rviz文件如下

Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded: ~
Splitter Ratio: 0.616034
Tree Height: 469
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.03
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Class: rviz/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: true
turtle1:
Value: true
turtle2:
Value: true
world:
Value: true
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
{}
Update Interval: 0
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: world
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Angle: -5.96046e-08
Class: rviz/TopDownOrtho
Enable Stereo Rendering:
Stereo Eye Separation: 0.06
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Name: Current View
Near Clip Distance: 0.01
Scale: 48.8711
Target Frame: <Fixed Frame>
Value: TopDownOrtho (rviz)
X: -0.254099
Y: -0.280596
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 747
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd0000000400000000000000f100000260fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003600000260000000b700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000280000000160000000000000000000000010000010f000000e9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000036000000e90000009b00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000055400000035fc0100000002fb0000000800540069006d00650100000000000005540000024600fffffffb0000000800540069006d006501000000000000045000000000000000000000045d0000026000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1364
X: 0
Y: 19

3. 详细分析

1.turtle_tf_broadcaster.cpp代码分析

（1）主函数中订阅turtle1/pose消息，当有消息时，调用消息响应函数poseCallback。

ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

（2）消息响应函数poseCallback调用sendTransform方法广播消息/tf消息

如下

static tf::TransformBroadcaster br;
//***
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));

2.rviz文件的结构

结构如图