ROS TF2 ros - 锐客网

ROS TF2

介绍tf2

1、安装演示示例
2、运行演示示例
3、如何实现的
4、tf2 工具

4.1 view_frames
4.2 tf_echo
4.3 rviz

编写tf2静态广播器（C ++）

1、创建 learning_tf2 功能包
2、How to broadcast transforms

2.1 代码
2.2 代码解释

3、编译运行
4、检查结果

5、发布静态转换更合适的方法

编写 tf2 坐标变换（上面为静态，此为动态）

1、创建 learning_tf2 功能包
2、代码
3、代码解释

1）
2）
3）
4）
5）

4、运行例程

1）编译
2）为此bin文件创建启动文件（launch）。命名为start_demo.launch
3）运行
4）查看结果

官网教程链接
介绍tf2 turtlesim的多机器人示例中，展示了tf2的一些功能。也介绍了使用tf2_echo，view_frames和rviz。
使对tf2可以做的事情有个很好的了解。
1、安装演示示例首先，获取所有依赖项并编译演示包。

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

文章图片

完毕后如上图
2、运行演示示例现在，已经编译了turtle_tf2教程包，运行演示。

$ roslaunch turtle_tf2 turtle_tf2_demo.launch

有两只小乌龟，一只爬向了另一只

文章图片

运行键盘控制乌龟移动的节点

rosrun turtlesim turtle_teleop_key

通过箭头来移动一只乌龟，可以看到另一只乌龟一直追着。

文章图片

3、如何实现的使用tf2库创建三个坐标系：世界坐标系、turtle1坐标系和turtle2坐标系。使用tf2广播器发布turtle标系，并使用tf2侦听器计算turtle标系中的差异并移动一只乌龟跟随另一只乌龟。
4、tf2 工具 4.1 view_frames
view_frames 创建 tf2 通过ROS广播的所有坐标系的示意图，他们的相互关系。

$ rosrun tf2_tools view_frames.py

文章图片
tf2侦听器正在侦听通过ROS广播的坐标系，并绘制坐标系连接方式的树。要查看树：
可以通过指令

$ evince frames.pdf

文章图片

这个指令会运行然后自动结束的。直接给你保存到home路径下，也不问一下。。。
在这个图上，可以看到tf2广播的三个坐标系：世界坐标系，turtle1坐标系和turtle2坐标系，并且世界坐标系是turtle1和turtle2坐标系的父级。 view_frames还报告一些诊断信息，这些信息有关何时接收到最旧和最新的坐标系转换，以及将tf2帧发布到tf2进行调试的速度。
4.2 tf_echo
tf_echo报告通过ROS广播的任何两个坐标系之间的转换关系
指令格式

$ rosrun tf tf_echo [reference_frame] [target_frame]

看一下turtle2坐标系相对于turtle1坐标系的变换

$ rosrun tf tf_echo turtle1 turtle2

未运动时

文章图片
移动时

文章图片

当移动乌龟时，会看到随着两只乌龟相对移动，坐标变换发生了变化。
4.3 rviz
rviz是一种可视化工具，可用于检查tf2坐标系。看看通过rviz的turtle坐标系。让我们使用turtle_tf2配置文件，使用rviz的-d选项启动rviz：

$ rosrun rviz rviz -d `rospack find turtle_tf2`/rviz/turtle_rviz.rviz

文章图片
控制乌龟移动，会看到网格中的两个坐标系也动了，并且一个跟随一个。
编写tf2静态广播器（C ++）下面记录如何将静态坐标系广播到tf2
之后编写代码实现上面的演示示例
1、创建 learning_tf2 功能包 【ROS TF2】首先，将创建一个catkin功能包，该功能包将用于学习tf2。这个名为learning_tf2的软件包依赖tf2，tf2_ros，roscpp，rospy和turtlesim。
指令：

$ catkin_create_pkg learning_tf2 tf2 tf2_ros roscpp rospy turtlesim

2、How to broadcast transforms 2.1 代码
创建一个 cpp src/static_turtle_tf2_broadcaster.cpp

#include #include #include #include #include std::string static_turtle_name; int main(int argc, char **argv) { ros::init(argc,argv, "my_static_tf2_broadcaster"); //初始化ros节点// 判断参数对不对参数应该有8个 if(argc != 8) { ROS_ERROR("Invalid number of parameters\nusage: static_turtle_tf2_broadcaster child_frame_name x y z roll pitch yaw"); return -1; } if(strcmp(argv[1],"world")==0)// 子坐标系不能是 world { ROS_ERROR("Your static turtle name cannot be 'world'"); return -1; }static_turtle_name = argv[1]; // 取得静态乌龟坐标系的名字static tf2_ros::StaticTransformBroadcaster static_broadcaster; //创建了一个 StaticTransformBroadcaster 对象之后通过这个来发布转换信息geometry_msgs::TransformStamped static_transformStamped; // 声明转换信息// 赋值转换信息 static_transformStamped.header.stamp = ros::Time::now(); static_transformStamped.header.frame_id = "world"; static_transformStamped.child_frame_id = static_turtle_name; static_transformStamped.transform.translation.x = atof(argv[2]); static_transformStamped.transform.translation.y = atof(argv[3]); static_transformStamped.transform.translation.z = atof(argv[4]); tf2::Quaternion quat; quat.setRPY(atof(argv[5]), atof(argv[6]), atof(argv[7])); static_transformStamped.transform.rotation.x = quat.x(); static_transformStamped.transform.rotation.y = quat.y(); static_transformStamped.transform.rotation.z = quat.z(); static_transformStamped.transform.rotation.w = quat.w(); // 通过StaticTransformBroadcaster把转换信息发送出去 static_broadcaster.sendTransform(static_transformStamped); // 终端显示 ROS_INFO("Spinning until killed publishing %s to world", static_turtle_name.c_str()); ros::spin(); return 0; };

2.2 代码解释
包含发布静态坐标转换需要的文件

#include #include // 这个是核心 #include #include #include

判断参数对不对参数应该有8个

// 判断参数对不对参数应该有8个 if(argc != 8) { ROS_ERROR("Invalid number of parameters\nusage: static_turtle_tf2_broadcaster child_frame_name x y z roll pitch yaw"); return -1; } if(strcmp(argv[1],"world")==0)// 子坐标系不能是 world { ROS_ERROR("Your static turtle name cannot be 'world'"); return -1; }

声明一些内容

static_turtle_name = argv[1]; // 取得静态乌龟坐标系的名字static tf2_ros::StaticTransformBroadcaster static_broadcaster; //创建了一个 StaticTransformBroadcaster 对象之后通过这个来发布转换信息geometry_msgs::TransformStamped static_transformStamped; // 声明转换信息

赋值转换信息

// 赋值转换信息 static_transformStamped.header.stamp = ros::Time::now(); static_transformStamped.header.frame_id = "world"; static_transformStamped.child_frame_id = static_turtle_name; static_transformStamped.transform.translation.x = atof(argv[2]); static_transformStamped.transform.translation.y = atof(argv[3]); static_transformStamped.transform.translation.z = atof(argv[4]); tf2::Quaternion quat; quat.setRPY(atof(argv[5]), atof(argv[6]), atof(argv[7])); static_transformStamped.transform.rotation.x = quat.x(); static_transformStamped.transform.rotation.y = quat.y(); static_transformStamped.transform.rotation.z = quat.z(); static_transformStamped.transform.rotation.w = quat.w();

通过 StaticTransformBroadcaster 把转换信息发送出去

// 通过StaticTransformBroadcaster把转换信息发送出去 static_broadcaster.sendTransform(static_transformStamped);

可以看到 main函数里没有 while 循环，所以只执行一次。
相当于设定的一个静态（不变）的坐标系和世界坐标系的转换（TF）
3、编译运行在 CMakeLists.txt 添加

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

编译

$ catkin_make

运行

$ roscore

rosrun learning_tf2 static_turtle_tf2_broadcaster mystaticturtle 0 0 1 0 0 0

4、检查结果

$ rostopic echo /tf_static

结果

文章图片
/tf_static 这个topic 可能会存放所有通过 StaticTransformBroadcaster 静态坐标转换
5、发布静态转换更合适的方法
上面的代码旨在说明如何使用StaticTransformBroadcaster发布静态转换。在实际的开发过程中，不必自己编写此代码，而应优先使用专用的tf2_ros工具来编写代码。 tf2_ros提供了一个名为static_transform_publisher的可执行文件，可以用作命令行工具或可以添加到启动文件的节点。
有两种方式一种角度的一种四元数的用有几个参数来区分

static_transform_publisher x y z yaw pitch roll frame_id child_frame_id

Publish a static coordinate transform to tf2 using an x/y/z offset in meters and quaternion.

通过这中 launch 文件的方式，直接在launch 中发布静态坐标变化

编写 tf2 坐标变换（上面为静态，此为动态）上面为静态：两坐标变换关系不变
此为动态：两坐标变换关系改变
下面记录如何将坐标系广播到tf2。在这种情况下，广播海龟移动时不断变化的坐标系。
1、创建 learning_tf2 功能包首先，将创建一个catkin功能包，该功能包将用于学习tf2。这个名为learning_tf2的软件包依赖tf2，tf2_ros，roscpp，rospy和turtlesim。
指令：

$ catkin_create_pkg learning_tf2 tf2 tf2_ros roscpp rospy turtlesim

2、代码

#include #include #include #include #include std::string turtle_name; void poseCallback(const turtlesim::PoseConstPtr& msg){ 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; 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(); 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")) { if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1; }; turtle_name = argv[1]; } else { private_node.getParam("turtle", turtle_name); }ros::NodeHandle node; ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback); ros::spin(); return 0; };

3、代码解释 1）

#include #include #include #include #include

tf2 软件包提供了 TransformBroadcaster 的实现，以帮助简化发布转换的任务。要使用TransformBroadcaster，需要包含tf2_ros / transform_broadcaster.h头文件。
2）

static tf2_ros::TransformBroadcaster br;

创建一个TransformBroadcaster对象，稍后将使用它发送tf。
3）

geometry_msgs::TransformStamped transformStamped; transformStamped.header.stamp = ros::Time::now(); transformStamped.header.frame_id = "world"; transformStamped.child_frame_id = turtle_name;

创建一个Transform对象，并为其提供适当的数据。
给要发布的tf加一个时间戳，用当前时间ros :: Time :: now（）标记它。
设置要创建的link的父框架的名称，在本例中为“ world”
设置要创建的link的子节点的名称，这就是乌龟本身的名称。
4）

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乌龟位姿信息复制到3D变换（TF）中
5）