ROS笔记-tf2的介绍

首先，安装一个tf2的演示demo

$ sudo apt-get install ros-${ROS_DISTRO}-turtle-tf2 ros-${ROS_DISTRO}-tf2-tools ros-${ROS_DISTRO}-tf

通过如下指令运行演示demo

roslaunch turtle_tf2 turtle_tf2_demo.launch

正常情况会报错，提示“/usr/bin/env: 'python' : No such file or directory”

因为系统环境是20.04LTS+noetic，按照步骤装ros的时候装的是python3

解决方法：

运行命令 whereis python3

sudo ln -s /usr/bin/python3 /usr/bin/python

再运行roslaunch就能正常运行demo，这个demo就是一只小龟追自己操作的另外一只小龟，主要关注其具体的实现原理

通过rospack find turtle_tf2 找到turtle_tf2包的位置，进去找到turtle_tf2_demo.launch

 根据launch文件可以看到

• 启动了turtlesim包里的两个节点，sim和teleop，节点类型分别为turtlesim_node、turtle_teleop_key，一般来说，node语句中的type表示节点类型或者同名的可执行文件，name表示节点名

• param 如果在node语句中则表示为/node_name/param_name，如该launch文件中虽然两个param的name都是turtle，但是因为在不同的node中，因此实际的参数名应该分别为：/turtle1_tf2_broadcaster/turtle和/turtle2_tf2_broadcaster/turtle，所以并不影响，他的value表示为参数源

• 其他的类似启动节点或者定义参数都算好理解

介绍两个tf2的工具，view_frames和tf_echo

view_frames 用法：$ rosrun tf2_tools view_frames.py

tf_echo 用法：rosrun tf tf_echo [reference_frame] [target_frame]

具体解释在http://wiki.ros.org/tf2/Tutorials/Introduction%20to%20tf2上，小工具，先了解

用c++实现一个tf2 broadcaster

先看头文件 #include <tf2_ros/transform_broadcaster.h> 

tf2包中有transformbroadcaster这样一个广播器，可以更方便的广播位姿变换，要使用这个广播器头文件里面就需要包含/tf2_ros/transform_broadcaster.h

static tf2_ros::TransformBroadcaster br;

创建了一个TransformBroadcaster对象br 稍后会用它来发送位姿变换

geometry_msgs::TransformStamped transformStamped;

transformStamped.header.stamp = ros::Time::now();

transformStamped.header.frame_id = "world";

transformStamped.child_frame_id = turtle_name;

这里的教程原文解释是 Here we create a Transform object and give it the appropriate metadata.

1. We need to give the transform being published a timestamp（时间戳）,we'll just stamp it with the current time, ros::Time::now()

2. Then, we need to set the name of the parent frame of the link we're creating, in this case "world"

3. Finally, we need to set the name of the child node of the link we're creating, in this case this is the name of the turtle itself

接着是

transformStamped.transform.translation.x = msg->x;

transformStamped.transform.translation.y = msg->y;

transformStamped.transform.translation.z = 0.0;

tf2::Quaternion q;

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();

这里表示将3D turtle pose的信息复制到3D transform中去 

最后通过

br.sendTransform(transformStamped);

用br将变换发送出去

Note: sendTransform and StampedTransform have opposite ordering of parent and child.

Note2: you can also publish static transforms on the same pattern by instantiating a StaticTransformBroadcaster instead of a TransfomBroadcaster.（详细展开在：http://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20static%20broadcaster%20%28C%2B%2B%29）

main函数里的内容教程中没有分析，直接到下一步运行这个broadcaster，当写好一个节点.cpp文件之后，首先需要在包里的CMakeLists.txt文件中为这个.cpp文件添加如下项

add_executable(turtle_tf2_broadcaster src/turtle_tf2_broadcaster.cpp)

target_link_libraries(turtle_tf2_broadcaster ${catkin_LIBRARIES})

然后返回catkin_ws文件夹进行catkin_make

当文件编译好了之后下一步就是创建对应的launch文件

然后同样在CMakeList.txt文件中加入

## Mark other files for installation (e.g. launch and bag files, etc.)

install(FILES start_demo.launch # myfile2 

DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

完成上述步骤后就可以在终端运行

roslaunch learning_tf2 start_demo.launch

用c++实现一个tf2 listener

首先是创建源文件 src/turtle_tf2_listener.cpp

代码解析

头文件 #include <tf2_ros/transform_listener.h>

tf2包提供了TransformListener来使得位姿变换的接收更加方便，因此要使用TransformListener，我们需要包含对应的头文件

tf2_ros::Buffer  tfBuffer;

tf2_ros::TransformListener  tfListener(tfBuffer);

这里创建了一个TransformListener对象，当Listener创建好后，它就会通过网络来接收tf2的位姿变换信息，and buffers them for up to 10 seconds.（并将其缓冲长达10s：我的理解是将接收到的信息最多能缓存10s）

The TransformListener object should be scoped to persist otherwise its cache will be unable to fill and almost every query will fail. A common method is to make the TransformListener object a member variable of a class.（TransformListener对象的范围应该是持久化的，否则它的缓存将无法填充，几乎所有的查询都会失败，一个常见的方法是让该对象成为一个类别的成员变量。ps:机翻，并没有看太懂）

try{   

    transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time(0)); } 

catch (tf2::TransformException &ex) {   

    ROS_WARN("%s",ex.what());   

    ros::Duration(1.0).sleep();   

    continue; 

}

Here, the real work is done, we query the listener for a specific transformation. Let's take a look at the four arguments （关于lookup Transform()的解释）

1. We want the tansform to this frame (target frame)...

2. ...from this frame (source frame)

3.  The time at which we want to transform. Providing ros::Time(0) will just get us the latest available transform

4. Duration before timeout. (optinal, default = ros::Duration(0,0))

接下来是运行该代码，与之前的broadcaster.cpp类似，需要先对其进行编译

在CMakeList.txt中加入

add_executable(turtle_tf2_listener src/turtle_tf2_listener.cpp)

target_link_libraries(turtle_tf2_listener ${catkin_LIBRARIES})

在start_demo.launch文件中加入

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

最后运行

roslaunch learning_tf2 start_demo.launch

（对于broadcaster和listener这两个cpp文件只能说是了解了个大概，还是有一些不太明白的地方，先暂且放在这里，以后用到再细看吧，关于tf2还有一些后续的教程在 http://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20listener%20%28C%2B%2B%29 ）