tf2 static broadcaster
Kong Liangqian Lv6
 2021-08-23 09:59:22  

功能包总览

发布静态转换对于定义机器人底座及其传感器或非移动部件之间的关系非常有用。例如，激光扫描测量在激光扫描仪的中心的时候是最好运算的

本节内容分为俩部分，一部分是关于如何通过写代码的方式来发布静态变换，另一部分是在命令行使用tf2_ros中的static_transform_publisher发布静态变换

tf2可以用用户随时追溯多个坐标系，tf2会实时的把各个坐标系之间的关系保存在一个树结构中。我们可以随时获得任何时间任何坐标系的变换

创建static broadcaster

首先，进入工程目录的src文件夹

1
ros2 pkg create --build-type ament_python learning_tf2_py

在src/learning_tf2_py/learning_tf2_py文件夹里面，我们加入以下代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
import sys

from geometry_msgs.msg import TransformStamped

import rclpy
from rclpy.node import Node

from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster

import tf_transformations


class StaticFramePublisher(Node):
   """
   Broadcast transforms that never change.

   This example publishes transforms from `world` to a static turtle frame.
   The transforms are only published once at startup, and are constant for all
   time.
   """

   def __init__(self, transformation):
      super().__init__('static_turtle_tf2_broadcaster')

      self._tf_publisher = StaticTransformBroadcaster(self)

      # Publish static transforms once at startup
      self._tf_publisher.sendTransform(self.make_transforms(transformation))

   def make_transforms(self, transformation):
      static_transformStamped = TransformStamped()
      static_transformStamped.header.stamp = self.get_clock().now().to_msg()
      static_transformStamped.header.frame_id = 'world'
      static_transformStamped.child_frame_id = sys.argv[1]
      static_transformStamped.transform.translation.x = float(sys.argv[2])
      static_transformStamped.transform.translation.y = float(sys.argv[3])
      static_transformStamped.transform.translation.z = float(sys.argv[4])
      quat = tf_transformations.quaternion_from_euler(
            float(sys.argv[5]), float(sys.argv[6]), float(sys.argv[7]))
      static_transformStamped.transform.rotation.x = quat[0]
      static_transformStamped.transform.rotation.y = quat[1]
      static_transformStamped.transform.rotation.z = quat[2]
      static_transformStamped.transform.rotation.w = quat[3]

      return (static_transformStamped)


def main():
   logger = rclpy.logging.get_logger('logger')

   # obtain parameters from command line arguments
   if len(sys.argv) < 8:
      logger.info('Invalid number of parameters. Usage: \n'
                  '$ ros2 run learning_tf2_py static_turtle_tf2_broadcaster'
                  'child_frame_name x y z roll pitch yaw')
      sys.exit(0)
   else:
      if sys.argv[1] == 'world':
            logger.info('Your static turtle name cannot be "world"')
            sys.exit(0)

   # pass parameters and initialize node
   rclpy.init()
   node = StaticFramePublisher(sys.argv)
   try:
      rclpy.spin(node)
   except KeyboardInterrupt:
      pass

   rclpy.shutdown()

代码审查

现在让我们看看与将静态海龟姿势发布到 tf2 相关的代码。第一行，我们引入了TransformStamped，这为我们发布消息到tf树提供了模板

1
from geometry_msgs.msg import TransformStamped

下面引入了Node

1
2
import rclpy
from rclpy.node import Node

tf2_ros包提供了StaticTransformBroadcaster使得发布静态的变换变得容易，

tf_transformations提供了从欧拉角到四元数的转换

1
2
3
from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster

import tf_transformations

StaticFramePublisher定义了结点名字为static_turtle_tf2_broadcaster，StaticTransformBroadcaster被构建，一旦程序开始，他就开始发送变换

1
2
3
4
5
6
super().__init__('static_turtle_tf2_broadcaster')

self._tf_publisher = StaticTransformBroadcaster(self)

# Publish static transforms once at startup
self._tf_publisher.sendTransform(self.make_transforms(transformation))

在这里，我们创建了一个 TransformStamped 对象，它将是我们将在填充后发送的消息，在给他实际的转换内容之前，还需要基于一些元数据信息

  1. 我们需要给正在发布的transform一个时间戳，我们只需用当前时间标记它，get_clock().now()
  2. 我们需要设置他的parent frame, 在这里是world
  3. 最后我们需要设置child frame的名字
1
2
3
4
static_transformStamped = TransformStamped()
static_transformStamped.header.stamp = self.get_clock().now().to_msg()
static_transformStamped.header.frame_id = 'world'
static_transformStamped.child_frame_id = sys.argv[1]

在这里我们提出了小乌龟的6D姿态（平移，旋转）

1
2
3
4
5
6
7
8
9
static_transformStamped.transform.translation.x = float(sys.argv[2])
static_transformStamped.transform.translation.y = float(sys.argv[3])
static_transformStamped.transform.translation.z = float(sys.argv[4])
quat = tf_transformations.quaternion_from_euler(
   float(sys.argv[5]), float(sys.argv[6]), float(sys.argv[7]))
static_transformStamped.transform.rotation.x = quat[0]
static_transformStamped.transform.rotation.y = quat[1]
static_transformStamped.transform.rotation.z = quat[2]
static_transformStamped.transform.rotation.w = quat[3]

最后我们利用sendTransform发出静态变换

1
broadcaster.sendTransform(static_transformStamped)

package.xml

添加执行依赖，打开package.xml，添加如下依赖

1
2
3
4
5
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>rclpy</exec_depend>
<exec_depend>tf_transformations</exec_depend>
<exec_depend>tf2_ros</exec_depend>
<exec_depend>turtlesim</exec_depend>

添加一个结点进入点

打开setup.py文件，在console_scripts下添加

1
'static_turtle_tf2_broadcaster = learning_tf2_py.static_turtle_tf2_broadcaster:main'

编译运行

安装依赖

1
rosdep install -i --from-path src --rosdistro foxy -y

build

1
colcon build --packages-select learning_tf2_py

source，执行

1
2
. install/setup.bash
ros2 run learning_tf2_py static_turtle_tf2_broadcaster mystaticturtle 0 0 1 0 0 0

这里设置了一个距离地面1米的一个位姿

我们现在可以通过打印出tf_static topic内容来检查这个发布出来的静态变换

若是一切正常，应该可以看见如下信息

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
transforms:
- header:
   stamp:
      sec: 1622908754
      nanosec: 208515730
   frame_id: world
child_frame_id: mystaticturtle
transform:
   translation:
      x: 0.0
      y: 0.0
      z: 1.0
   rotation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0

合适的方式发布静态变换

命令行

以上的内容应该在今后的开发过程中，使用tf2_ros来做静态的坐标变换，命令行可以使用欧拉角，平移的方式进行发布

1
ros2 run tf2_ros static_transform_publisher x y z yaw pitch roll frame_id child_frame_id

或者使用四元数、平移的方式进行发布

1
ros2 run tf2_ros static_transform_publisher x y z qx qy qz qw frame_id child_frame_id

launch文件

1
2
3
4
5
6
7
8
9
10
11
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
   return LaunchDescription([
      Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            arguments = ['0', '0', '1', '0', '0', '0', 'world', 'mystaticturtle']
      ),
   ])

c++ 版本

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
#include <geometry_msgs/msg/transform_stamped.hpp>

#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/static_transform_broadcaster.h>

#include <memory>

using std::placeholders::_1;

class StaticFramePublisher : public rclcpp::Node
{
public:
  explicit StaticFramePublisher(char * transformation[])
  : Node("static_turtle_tf2_broadcaster")
  {
    tf_publisher_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);

    // Publish static transforms once at startup
    this->make_transforms(transformation);
  }

private:
  void make_transforms(char * transformation[])
  {
    rclcpp::Time now;
    geometry_msgs::msg::TransformStamped t;

    t.header.stamp = now;
    t.header.frame_id = "world";
    t.child_frame_id = transformation[1];

    t.transform.translation.x = atof(transformation[2]);
    t.transform.translation.y = atof(transformation[3]);
    t.transform.translation.z = atof(transformation[4]);
    tf2::Quaternion q;
    q.setRPY(
      atof(transformation[5]),
      atof(transformation[6]),
      atof(transformation[7]));
    t.transform.rotation.x = q.x();
    t.transform.rotation.y = q.y();
    t.transform.rotation.z = q.z();
    t.transform.rotation.w = q.w();

    tf_publisher_->sendTransform(t);
  }
  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_publisher_;
};

int main(int argc, char * argv[])
{
  auto logger = rclcpp::get_logger("logger");

  // Obtain parameters from command line arguments
  if (argc != 8) {
    RCLCPP_INFO(
      logger, "Invalid number of parameters\nusage: "
      "ros2 run learning_tf2_cpp static_turtle_tf2_broadcaster "
      "child_frame_name x y z roll pitch yaw");
    return 1;
  }

  // As the parent frame of the transform is `world`, it is
  // necessary to check that the frame name passed is different
  if (strcmp(argv[1], "world") == 0) {
    RCLCPP_INFO(logger, "Your static turtle name cannot be 'world'");
    return 1;
  }

  // Pass parameters and initialize node
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<StaticFramePublisher>(argv));
  rclcpp::shutdown();
  return 0;
}

我们include了 geometry_msgs/msg/transform_stamped.hpp 来使用发布到tf树的 TransformStamped 

1
#include <geometry_msgs/msg/transform_stamped.hpp>

rclcpp已经包含了Node类

1
#include <rclcpp/rclcpp.hpp>

​ tf2::Quaternion 可以方便的进行欧拉角和四元数的相互转换。引入 tf2_ros/static_transform_broadcaster.h 可以使用 StaticTransformBroadcaster ，这使得静态的变换变得容易

1
2
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/static_transform_broadcaster.h>

其余的逻辑和Python一样。

package.xml

完善<description>, <maintainer> and <license> 标签后:

1
2
3
4
5
<depend>geometry_msgs</depend>
<depend>rclcpp</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>turtlesim</depend>

CMakeLists.txt

在find_package(ament_cmake REQUIRED)的后面，添加

1
2
3
4
5
find_package(geometry_msgs REQUIRED)
find_package(rclcpp REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(turtlesim REQUIRED)

添加结点，这样才可以ros2 run，并且进行安装

1
2
3
4
5
6
7
8
9
10
11
12
add_executable(static_turtle_tf2_broadcaster src/static_turtle_tf2_broadcaster.cpp)
ament_target_dependencies(
   static_turtle_tf2_broadcaster
   geometry_msgs
   rclcpp
   tf2
   tf2_ros
   turtlesim
)
install(TARGETS
   static_turtle_tf2_broadcaster
   DESTINATION lib/${PROJECT_NAME})
 Comments