[ROS2基础] TF2使用细节

机器人系统ros2-开发实践05-ROS2 中 tf2的定义及示例说明 tf2的全称是transform2,在ROS(Robot Operating System)中,它是专门用于处理和变换不同坐标系间位置和方向的库。tf2是ROS2中一个重要的组件,因为在机器人的许多应用中,正确理解各个部件如何相对于彼此移动是至关重要的。一旦turtlesim启动,您可以使用键盘箭头键在turtlesim中驱动中央乌龟,选择第二个终端窗口,以便捕获您的击键来驱动乌龟。在侧栏中,您将看到 tf2 广播的帧。tf2_echo当侦听器接收通过 ROS 2 广播的帧时,您将看到显示的转换。 阅读详情

运行一个示例

安装依赖
sudo apt-get install ros-galactic-turtle-tf2-py ros-galactic-tf2-tools ros-galactic-tf-transformations
pip3 install transforms3d
运行示例

在不同的命令窗口中运行下面的命令

启动小乌龟窗口

ros2 launch turtle_tf2_py turtle_tf2_demo.launch.py

启动键盘控制节点

ros2 run turtlesim turtle_teleop_key

观察坐标转换的结果

ros2 run tf2_ros tf2_echo turtle2 turtle1
示例分析

本示例中启动了两只小乌龟Turtle1和Turtle2。TF发布器会将Turtle1相对于world坐标系的位置关系和Turtle2相对于world坐标系的位置关系发布出来。为了实现Turtle2跟随Turtle1的效果,程序中获取了Turtle1相对于Turtle2的位置关系并且将其折算成速度控制量。

        // Look up for the transformation between target_frame and turtle2 frames
        // and send velocity commands for turtle2 to reach target_frame
        try {
          transformStamped = tf_buffer_->lookupTransform(
            toFrameRel, fromFrameRel,
            tf2::TimePointZero);//这里得到的是from_frame_rel在to_frame_rel坐标系下的相对位置
        } catch (tf2::TransformException & ex) {
          RCLCPP_INFO(
            this->get_logger(), "Could not transform %s to %s: %s",
            toFrameRel.c_str(), fromFrameRel.c_str(), ex.what());
          return;
        }

代码来自ros2_galactic_turorials/geometry_tutorials/turtle_tf2_cpp/src/turtle_tf2_listener.cpp。

上面的示例代码可通过下面的方式获取:

git clone https://gitee.com/shoufei/ros2_galactic_turorials.git

或者

git clone git@github.com:shoufei403/ros2_galactic_tutorials.git

TF2树

tf2树表征了各个link的位置关系。一个link有且只有一个parent link。所以tf2树是不会形成闭环的。

ros2 run tf2_tools view_frames

用这个命令可以保存当前系统中tf2树的关系图(以pdf文件的形式保存在运行命令的目录下)。

静态TF发布器和动态TF发布器

静态TF发布器
编写代码发布TF(C++)

示例代码来自ros2_galactic_turorials/geometry_tutorials/turtle_tf2_cpp/src/static_turtle_tf2_broadcaster.cpp

#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 = this->get_clock()->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;
}

编写代码代码发布TF(Python)

示例代码来自ros2_galactic_turorials/geometry_tutorials/turtle_tf2_py/turtle_tf2_py/turtle_tf2_broadcaster.py

from geometry_msgs.msg import TransformStamped

import rclpy
from rclpy.node import Node

from tf2_ros import TransformBroadcaster

import tf_transformations

from turtlesim.msg import Pose


class FramePublisher(Node):

    def __init__(self):
        super().__init__('turtle_tf2_frame_publisher')

        # Declare and acquire `turtlename` parameter
        self.declare_parameter('turtlename', 'turtle')
        self.turtlename = self.get_parameter(
            'turtlename').get_parameter_value().string_value

        # Initialize the transform broadcaster
        self.br = TransformBroadcaster(self)

        # Subscribe to a turtle{1}{2}/pose topic and call handle_turtle_pose
        # callback function on each message
        self.subscription = self.create_subscription(
            Pose,
            f'/{self.turtlename}/pose',
            self.handle_turtle_pose,
            1)


    def handle_turtle_pose(self, msg):
        t = TransformStamped()

        # Read message content and assign it to
        # corresponding tf variables
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = 'world'
        t.child_frame_id = self.turtlename

        # Turtle only exists in 2D, thus we get x and y translation
        # coordinates from the message and set the z coordinate to 0
        t.transform.translation.x = msg.x
        t.transform.translation.y = msg.y
        t.transform.translation.z = 0.0

        # For the same reason, turtle can only rotate around one axis
        # and this why we set rotation in x and y to 0 and obtain
        # rotation in z axis from the message
        q = tf_transformations.quaternion_from_euler(0, 0, msg.theta)
        t.transform.rotation.x = q[0]
        t.transform.rotation.y = q[1]
        t.transform.rotation.z = q[2]
        t.transform.rotation.w = q[3]

        # Send the transformation
        self.br.sendTransform(t)


def main():
    rclpy.init()
    node = FramePublisher()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    rclpy.shutdown()

手动以命令行形式发布TF

角度用欧拉角表示

x/y/z 偏移的单位是米,roll/pitch/yaw偏移的单位是rad。

Publish a static coordinate transform to tf2 using an x/y/z offset in meters and roll/pitch/yaw in radians. In our case, roll/pitch/yaw refers to rotation about the x/y/z-axis, respectively.

ros2 run tf2_ros 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.

ros2 run tf2_ros static_transform_publisher x y z qx qy qz qw frame_id child_frame_id
在launch文件中发布TF
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']
      ),
   ])
动态TF发布器

动态的tf发布一般是编写代码来实现的。

C++实现动态TF发布

示例代码来自ros2_galactic_turorials/geometry_tutorials/turtle_tf2_cpp/src/turtle_tf2_broadcaster.cpp

#include <geometry_msgs/msg/transform_stamped.hpp>

#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <turtlesim/msg/pose.hpp>

#include <memory>
#include <string>

using std::placeholders::_1;

class FramePublisher : public rclcpp::Node
{
public:
  FramePublisher()
  : Node("turtle_tf2_frame_publisher")
  {
    // Declare and acquire `turtlename` parameter
    this->declare_parameter<std::string>("turtlename", "turtle");
    this->get_parameter("turtlename", turtlename_);

    // Initialize the transform broadcaster
    tf_broadcaster_ =
      std::make_unique<tf2_ros::TransformBroadcaster>(*this);

    // Subscribe to a turtle{1}{2}/pose topic and call handle_turtle_pose
    // callback function on each message
    std::ostringstream stream;
    stream << "/" << turtlename_.c_str() << "/pose";
    std::string topic_name = stream.str();

    subscription_ = this->create_subscription<turtlesim::msg::Pose>(
      topic_name, 10,
      std::bind(&FramePublisher::handle_turtle_pose, this, _1));
  }

private:
  void handle_turtle_pose(const std::shared_ptr<turtlesim::msg::Pose> msg)
  {
    rclcpp::Time now = this->get_clock()->now();
    geometry_msgs::msg::TransformStamped t;

    // Read message content and assign it to
    // corresponding tf variables
    t.header.stamp = now;
    t.header.frame_id = "world";
    t.child_frame_id = turtlename_.c_str();

    // Turtle only exists in 2D, thus we get x and y translation
    // coordinates from the message and set the z coordinate to 0
    t.transform.translation.x = msg->x;
    t.transform.translation.y = msg->y;
    t.transform.translation.z = 0.0;

    // For the same reason, turtle can only rotate around one axis
    // and this why we set rotation in x and y to 0 and obtain
    // rotation in z axis from the message
    tf2::Quaternion q;
    q.setRPY(0, 0, msg->theta);
    t.transform.rotation.x = q.x();
    t.transform.rotation.y = q.y();
    t.transform.rotation.z = q.z();
    t.transform.rotation.w = q.w();

    // Send the transformation
    tf_broadcaster_->sendTransform(t);
  }
  rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr subscription_;
  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
  std::string turtlename_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<FramePublisher>());
  rclcpp::shutdown();
  return 0;
}

Python实现动态TF发布

示例代码来自ros2_galactic_turorials/geometry_tutorials/turtle_tf2_py/turtle_tf2_py/turtle_tf2_broadcaster.py

from geometry_msgs.msg import TransformStamped

import rclpy
from rclpy.node import Node

from tf2_ros import TransformBroadcaster

import tf_transformations

from turtlesim.msg import Pose


class FramePublisher(Node):

    def __init__(self):
        super().__init__('turtle_tf2_frame_publisher')

        # Declare and acquire `turtlename` parameter
        self.declare_parameter('turtlename', 'turtle')
        self.turtlename = self.get_parameter(
            'turtlename').get_parameter_value().string_value

        # Initialize the transform broadcaster
        self.br = TransformBroadcaster(self)

        # Subscribe to a turtle{1}{2}/pose topic and call handle_turtle_pose
        # callback function on each message
        self.subscription = self.create_subscription(
            Pose,
            f'/{self.turtlename}/pose',
            self.handle_turtle_pose,
            1)


    def handle_turtle_pose(self, msg):
        t = TransformStamped()

        # Read message content and assign it to
        # corresponding tf variables
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = 'world'
        t.child_frame_id = self.turtlename

        # Turtle only exists in 2D, thus we get x and y translation
        # coordinates from the message and set the z coordinate to 0
        t.transform.translation.x = msg.x
        t.transform.translation.y = msg.y
        t.transform.translation.z = 0.0

        # For the same reason, turtle can only rotate around one axis
        # and this why we set rotation in x and y to 0 and obtain
        # rotation in z axis from the message
        q = tf_transformations.quaternion_from_euler(0, 0, msg.theta)
        t.transform.rotation.x = q[0]
        t.transform.rotation.y = q[1]
        t.transform.rotation.z = q[2]
        t.transform.rotation.w = q[3]

        # Send the transformation
        self.br.sendTransform(t)


def main():
    rclpy.init()
    node = FramePublisher()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    rclpy.shutdown()

监听TF数据

实际上,就是是获取两个坐标系之间的相对位置。

C++版本

示例代码来自ros2_galactic_turorials/geometry_tutorials/turtle_tf2_cpp/src/turtle_tf2_listener.cpp

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

#include <rclcpp/rclcpp.hpp>
#include <tf2/exceptions.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <turtlesim/srv/spawn.hpp>

#include <chrono>
#include <memory>
#include <string>

using std::placeholders::_1;
using namespace std::chrono_literals;

class FrameListener : public rclcpp::Node
{
public:
  FrameListener()
  : Node("turtle_tf2_frame_listener"),
    turtle_spawning_service_ready_(false),
    turtle_spawned_(false)
  {
    // Declare and acquire `target_frame` parameter
    this->declare_parameter<std::string>("target_frame", "turtle1");
    this->get_parameter("target_frame", target_frame_);

    tf_buffer_ =
      std::make_unique<tf2_ros::Buffer>(this->get_clock());
    transform_listener_ =
      std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

    // Create a client to spawn a turtle
    spawner_ =
      this->create_client<turtlesim::srv::Spawn>("spawn");

    // Create turtle2 velocity publisher
    publisher_ =
      this->create_publisher<geometry_msgs::msg::Twist>("turtle2/cmd_vel", 1);

    // Call on_timer function every second
    timer_ = this->create_wall_timer(
      1s, std::bind(&FrameListener::on_timer, this));
  }

private:
  void on_timer()
  {
    // Store frame names in variables that will be used to
    // compute transformations
    std::string fromFrameRel = target_frame_.c_str();
    std::string toFrameRel = "turtle2";

    if (turtle_spawning_service_ready_) {
      if (turtle_spawned_) {
        geometry_msgs::msg::TransformStamped transformStamped;

        // Look up for the transformation between target_frame and turtle2 frames
        // and send velocity commands for turtle2 to reach target_frame
        try {
          transformStamped = tf_buffer_->lookupTransform(
            toFrameRel, fromFrameRel,
            tf2::TimePointZero);//这里得到的是from_frame_rel在to_frame_rel坐标系下的位置
        } catch (tf2::TransformException & ex) {
          RCLCPP_INFO(
            this->get_logger(), "Could not transform %s to %s: %s",
            toFrameRel.c_str(), fromFrameRel.c_str(), ex.what());
          return;
        }

        geometry_msgs::msg::Twist msg;

        static const double scaleRotationRate = 1.0;
        msg.angular.z = scaleRotationRate * atan2(
          transformStamped.transform.translation.y,
          transformStamped.transform.translation.x);

        static const double scaleForwardSpeed = 0.5;
        msg.linear.x = scaleForwardSpeed * sqrt(
          pow(transformStamped.transform.translation.x, 2) +
          pow(transformStamped.transform.translation.y, 2));

        publisher_->publish(msg);
      } else {
        RCLCPP_INFO(this->get_logger(), "Successfully spawned");
        turtle_spawned_ = true;
      }
    } else {
      // Check if the service is ready
      if (spawner_->service_is_ready()) {
        // Initialize request with turtle name and coordinates
        // Note that x, y and theta are defined as floats in turtlesim/srv/Spawn
        auto request = std::make_shared<turtlesim::srv::Spawn::Request>();
        request->x = 4.0;
        request->y = 2.0;
        request->theta = 0.0;
        request->name = "turtle2";

        // Call request
        using ServiceResponseFuture =
          rclcpp::Client<turtlesim::srv::Spawn>::SharedFuture;
        auto response_received_callback = [this](ServiceResponseFuture future) {
            auto result = future.get();
            if (strcmp(result->name.c_str(), "turtle2") == 0) {
              turtle_spawning_service_ready_ = true;
            } else {
              RCLCPP_ERROR(this->get_logger(), "Service callback result mismatch");
            }
          };
        auto result = spawner_->async_send_request(request, response_received_callback);
      } else {
        RCLCPP_INFO(this->get_logger(), "Service is not ready");
      }
    }
  }
  // Boolean values to store the information
  // if the service for spawning turtle is available
  bool turtle_spawning_service_ready_;
  // if the turtle was successfully spawned
  bool turtle_spawned_;
  rclcpp::Client<turtlesim::srv::Spawn>::SharedPtr spawner_{nullptr};
  rclcpp::TimerBase::SharedPtr timer_{nullptr};
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_{nullptr};
  std::shared_ptr<tf2_ros::TransformListener> transform_listener_{nullptr};
  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
  std::string target_frame_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<FrameListener>());
  rclcpp::shutdown();
  return 0;
}

增加超时时间的写法

        try {
          rclcpp::Time now = this->get_clock()->now();
          transformStamped = tf_buffer_->lookupTransform(
            toFrameRel,
            fromFrameRel,
            now,
            50ms);//获取当前时间的tf关系并且允许50ms的超时时间
        } catch (tf2::TransformException & ex) {
          RCLCPP_INFO(
            this->get_logger(), "Could not transform %s to %s: %s",
            toFrameRel.c_str(), fromFrameRel.c_str(), ex.what());
          return;
        }

示例代码来自ros2_galactic_turorials/geometry_tutorials/turtle_tf2_cpp/src/turtle_tf2_listener_timeout.cpp

可运行下面的命令查看效果

ros2 launch turtle_tf2_cpp turtle_tf2_demo_timeout.launch.py
ros2 run turtlesim turtle_teleop_key

获取历史时间点的tf关系

        try {
          rclcpp::Time now = this->get_clock()->now();
          rclcpp::Time when = now - rclcpp::Duration(5, 0);
          transformStamped = tf_buffer_->lookupTransform(
              toFrameRel,
              now,
              fromFrameRel,
              when,
              "world",
              50ms);
        } catch (tf2::TransformException & ex) {
          RCLCPP_INFO(
            this->get_logger(), "Could not transform %s to %s: %s",
            toFrameRel.c_str(), fromFrameRel.c_str(), ex.what());
          return;
        }

示例代码来自ros2_galactic_turorials/geometry_tutorials/turtle_tf2_cpp/src/turtle_tf2_listener_time_travel.cpp

运行命令

ros2 launch turtle_tf2_cpp turtle_tf2_demo_time_travel.launch.py
ros2 run turtlesim turtle_teleop_key

python版本

示例代码来自ros2_galactic_turorials/geometry_tutorials/turtle_tf2_py/turtle_tf2_py/turtle_tf2_listener.py

import math

from geometry_msgs.msg import Twist

import rclpy
from rclpy.node import Node

from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener

from turtlesim.srv import Spawn


class FrameListener(Node):

    def __init__(self):
        super().__init__('turtle_tf2_frame_listener')

        # Declare and acquire `target_frame` parameter
        self.declare_parameter('target_frame', 'turtle1')
        self.target_frame = self.get_parameter(
            'target_frame').get_parameter_value().string_value

        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)

        # Create a client to spawn a turtle
        self.spawner = self.create_client(Spawn, 'spawn')
        # Boolean values to store the information
        # if the service for spawning turtle is available
        self.turtle_spawning_service_ready = False
        # if the turtle was successfully spawned
        self.turtle_spawned = False

        # Create turtle2 velocity publisher
        self.publisher = self.create_publisher(Twist, 'turtle2/cmd_vel', 1)

        # Call on_timer function every second
        self.timer = self.create_timer(1.0, self.on_timer)

    def on_timer(self):
        # Store frame names in variables that will be used to
        # compute transformations
        from_frame_rel = self.target_frame
        to_frame_rel = 'turtle2'

        if self.turtle_spawning_service_ready:
            if self.turtle_spawned:
                # Look up for the transformation between target_frame and turtle2 frames
                # and send velocity commands for turtle2 to reach target_frame
                try:
                    now = rclpy.time.Time()
                    trans = self.tf_buffer.lookup_transform(
                        to_frame_rel,
                        from_frame_rel,
                        now)  #这里得到的是from_frame_rel在to_frame_rel坐标系下的位置
                except TransformException as ex:
                    self.get_logger().info(
                        f'Could not transform {to_frame_rel} to {from_frame_rel}: {ex}')
                    return

                msg = Twist()
                scale_rotation_rate = 1.0
                msg.angular.z = scale_rotation_rate * math.atan2(
                    trans.transform.translation.y,
                    trans.transform.translation.x)

                scale_forward_speed = 0.5
                msg.linear.x = scale_forward_speed * math.sqrt(
                    trans.transform.translation.x ** 2 +
                    trans.transform.translation.y ** 2)

                self.publisher.publish(msg)
            else:
                if self.result.done():
                    self.get_logger().info(
                        f'Successfully spawned {self.result.result().name}')
                    self.turtle_spawned = True
                else:
                    self.get_logger().info('Spawn is not finished')
        else:
            if self.spawner.service_is_ready():
                # Initialize request with turtle name and coordinates
                # Note that x, y and theta are defined as floats in turtlesim/srv/Spawn
                request = Spawn.Request()
                request.name = 'turtle2'
                request.x = float(4)
                request.y = float(2)
                request.theta = float(0)
                # Call request
                self.result = self.spawner.call_async(request)
                self.turtle_spawning_service_ready = True
            else:
                # Check if the service is ready
                self.get_logger().info('Service is not ready')


def main():
    rclpy.init()
    node = FrameListener()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    rclpy.shutdown()

增加超时时间的写法

trans = self._tf_buffer.lookup_transform(
   to_frame_rel,
   from_frame_rel,
   now,
   timeout=rclpy.time.Duration(seconds=1.0))

示例代码来自ros2_galactic_turorials/geometry_tutorials/turtle_tf2_py/turtle_tf2_py/turtle_tf2_listener_timeout.py

运行命令

ros2 launch turtle_tf2_py turtle_tf2_demo_timeout.launch.py
ros2 run turtlesim turtle_teleop_key

获取历史时间点的tf关系

                try:
                    when = self.get_clock().now() - rclpy.time.Duration(seconds=5.0)
                    trans = self.tf_buffer.lookup_transform_full(
                            target_frame=to_frame_rel,
                            target_time=rclpy.time.Time(),
                            source_frame=from_frame_rel,
                            source_time=when,
                            fixed_frame='world',
                            timeout=rclpy.time.Duration(seconds=0.05))
                except (LookupException, ConnectivityException, ExtrapolationException):
                    self.get_logger().info('transform not ready')
                    return

示例代码来自ros2_galactic_turorials/geometry_tutorials/turtle_tf2_py/turtle_tf2_py/turtle_tf2_listener_time_travel.py

运行命令

ros2 launch turtle_tf2_py turtle_tf2_demo_time_travel.launch.py
ros2 run turtlesim turtle_teleop_key

TF 调试工具

打印两个link的相对位置关系

ros2 run tf2_ros tf2_echo turtle2 turtle1

保存tf关系框图

ros2 run tf2_tools view_frames

监控两个link的转换延时

ros2 run tf2_ros tf2_monitor turtle2 turtle1

四元数与欧拉角转换

因为使用TF的过程中常常涉及到坐标转换。这会涉及到角度的表示方法。

通常我们会有两种表示角度的方法,一种是欧拉角,一种是四元数。欧拉角是更为直观的表示方法,但因为死锁的问题,在进行坐标变换计算时常常是使用四元数的。

在ROS2中,有两个数据类型表示四元数。它们是来自tf2功能包的tf2::Quaternion类型和来自geometry_msgs功能包的geometry_msgs::msg::Quaternion类型。我们可以通过tf2_geometry_msgs包来进行两个类型的互相转换。

C++的写法

#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
...

tf2::Quaternion tf2_quat, tf2_quat_from_msg;
tf2_quat.setRPY(roll, pitch, yaw);
// Convert tf2::Quaternion to geometry_msgs::msg::Quaternion
geometry_msgs::msg::Quaternion msg_quat = tf2::toMsg(tf2_quat);

// Convert geometry_msgs::msg::Quaternion to tf2::Quaternion
tf2::convert(msg_quat, tf2_quat_from_msg);
// or
tf2::fromMsg(msg_quat, tf2_quat_from_msg);

Python的写法

from geometry_msgs.msg import Quaternion
...

# Create a list of floats, which is compatible with tf2
# Quaternion methods
quat_tf = [0.0, 1.0, 0.0, 0.0]

# Convert a list to geometry_msgs.msg.Quaternion
msg_quat = Quaternion(x=quat_tf[0], y=quat_tf[1], z=quat_tf[2], w=quat_tf[3])

欧拉角和四元数之间的转换可以用下面的方式

欧拉角转四元数

C++版本

#include <tf2/LinearMath/Quaternion.h>

tf2::Quaternion myQuaternion;
myQuaternion.setRPY(1.5707, 0, -1.5707);  // Create this quaternion from roll/pitch/yaw (in radians)
ROS_INFO("%f  %f  %f  %f" ,myQuaternion.x(),myQuaternion.y(),myQuaternion.z(),myQuaternion.w());  // Print the quaternion components (0,0,0,1)

Python版本

import tf_transformations
...

q = tf_transformations.quaternion_from_euler(1.5707, 0, -1.5707)
print(f'The quaternion representation is x: {q[0]} y: {q[1]} z: {q[2]} w: {q[3]}.')

四元数转欧拉角

C++版本

在tf2/utils.h中定义了如下方法。

/** Return the yaw, pitch, roll of anything that can be converted to a tf2::Quaternion
 * The conventions are the usual ROS ones defined in tf2/LineMath/Matrix3x3.h
 * \param a the object to get data from (it represents a rotation/quaternion)
 * \param yaw yaw
 * \param pitch pitch
 * \param roll roll
 */
template<class A>
void getEulerYPR(const A & a, double & yaw, double & pitch, double & roll)
{
  tf2::Quaternion q = impl::toQuaternion(a);
  impl::getEulerYPR(q, yaw, pitch, roll);
}

/** Return the yaw of anything that can be converted to a tf2::Quaternion
 * The conventions are the usual ROS ones defined in tf2/LineMath/Matrix3x3.h
 * This function is a specialization of getEulerYPR and is useful for its
 * wide-spread use in navigation
 * \param a the object to get data from (it represents a rotation/quaternion)
 * \param yaw yaw
 */
template<class A>
double getYaw(const A & a)
{
  tf2::Quaternion q = impl::toQuaternion(a);
  return impl::getYaw(q);
}

Python版本

import tf_transformations

rpy = tf_transformations.euler_from_quaternion([0.06146124, 0, 0, 0.99810947])

更多细节请查看下面的文章:
https://docs.ros.org/en/galactic/Tutorials/Tf2/Quaternion-Fundamentals.html

tf2_ros::MessageFilter

https://docs.ros.org/en/galactic/Tutorials/Tf2/Using-Stamped-Datatypes-With-Tf2-Ros-MessageFilter.html

To do this turtle1 must listen to the topic where turtle3’s pose is being published, wait until transforms into the desired frame are ready, and then do its operations. To make this easier the tf2_ros::MessageFilter is very useful. The tf2_ros::MessageFilter will take a subscription to any ROS 2 message with a header and cache it until it is possible to transform it into the target frame.

tf2_ros::MessageFilter 的主要作用是,等待目标frame已经缓冲在tf树中。这样的话转换到该frame将是可行的。AMCL模块中处理激光数据就是一个好的例子。

void
AmclNode::initMessageFilters()
{
  laser_scan_sub_ = std::make_unique<message_filters::Subscriber<sensor_msgs::msg::LaserScan>>(
    rclcpp_node_.get(), scan_topic_, rmw_qos_profile_sensor_data);

  laser_scan_filter_ = std::make_unique<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>>(
    *laser_scan_sub_, *tf_buffer_, odom_frame_id_, 10, rclcpp_node_, transform_tolerance_);

  laser_scan_connection_ = laser_scan_filter_->registerCallback(
    std::bind(
      &AmclNode::laserReceived,
      this, std::placeholders::_1));
}

当odom_frame_id_在tf树中可转换时,接受到激光数据后开始执行回调函数laserReceived 。在laserReceived 中通过odom_frame_id_ 获取到当前的odom 数据。也就是说,使用tf2_ros::MessageFilter 达到了,等能获取odom 数据时再处理激光数据的效果。

下面是官方的一个示例。可以参考其写法。

#include <geometry_msgs/msg/point_stamped.hpp>
#include <message_filters/subscriber.h>

#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/create_timer_ros.h>
#include <tf2_ros/message_filter.h>
#include <tf2_ros/transform_listener.h>
#ifdef TF2_CPP_HEADERS
  #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#else
  #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#endif

#include <chrono>
#include <memory>
#include <string>

using namespace std::chrono_literals;

class PoseDrawer : public rclcpp::Node
{
public:
  PoseDrawer()
  : Node("turtle_tf2_pose_drawer")
  {
    // Declare and acquire `target_frame` parameter
    this->declare_parameter<std::string>("target_frame", "turtle1");
    this->get_parameter("target_frame", target_frame_);

    typedef std::chrono::duration<int> seconds_type;
    seconds_type buffer_timeout(1);

    tf2_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
    // Create the timer interface before call to waitForTransform,
    // to avoid a tf2_ros::CreateTimerInterfaceException exception
    auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
      this->get_node_base_interface(),
      this->get_node_timers_interface());
    tf2_buffer_->setCreateTimerInterface(timer_interface);
    tf2_listener_ =
      std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);

    point_sub_.subscribe(this, "/turtle3/turtle_point_stamped");
    tf2_filter_ = std::make_shared<tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped>>(
      point_sub_, *tf2_buffer_, target_frame_, 100, this->get_node_logging_interface(),
      this->get_node_clock_interface(), buffer_timeout);
    // Register a callback with tf2_ros::MessageFilter to be called when transforms are available
    tf2_filter_->registerCallback(&PoseDrawer::msgCallback, this);//等待需要的tf关系就绪,一就绪有数据就进入到回调函数中。
  }

private:
  void msgCallback(const geometry_msgs::msg::PointStamped::SharedPtr point_ptr)
  {
    geometry_msgs::msg::PointStamped point_out;
    try {
      tf2_buffer_->transform(*point_ptr, point_out, target_frame_);//point_out的值是turtle3想对于target_frame_的坐标
      RCLCPP_INFO(
        this->get_logger(), "Point of turtle3 in frame of turtle1: x:%f y:%f z:%f\n",
        point_out.point.x,
        point_out.point.y,
        point_out.point.z);
    } catch (tf2::TransformException & ex) {
      RCLCPP_WARN(
        // Print exception which was caught
        this->get_logger(), "Failure %s\n", ex.what());
    }
  }
  std::string target_frame_;
  std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
  std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
  message_filters::Subscriber<geometry_msgs::msg::PointStamped> point_sub_;
  std::shared_ptr<tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped>> tf2_filter_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<PoseDrawer>());
  rclcpp::shutdown();
  return 0;
}

参考:

https://docs.ros.org/en/galactic/Tutorials/Tf2/Tf2-Main.html


觉得有用就点赞吧!

我是首飞,一个帮大家填坑的机器人开发攻城狮。

另外在公众号《首飞》内回复“机器人”获取精心推荐的C/C++,Python,Docker,Qt,ROS1/2等机器人行业常用技术资料。

Ros2 TF工具 ROS2 TF工具使用 阅读详情

相关推荐

ROS2实战避坑指南:从环境配置到TF发布全链路解析

ROS2作为机器人操作系统的核心中间件,其本质是基于DDS的数据分发服务封装。理解其节点(Node)、执行器(Executor)与回调组(CallbackGroup)的进程/线程映射关系,是解决多线程阻塞、实时性抖动等工程问题的前提;而服务质量(QoS)策略如reliability和durability,则直接决定传感器数据可靠性与控制指令确定性。在真实部署中,跨平台兼容性(如WSL2网络命名空间、Apple Silicon架构适配)、Intra-Process零拷贝通信优化、自定义内存分配器等技术,已成为

weixin_33744141的博客 401

【ROS2机器人入门到实战】tf2介绍

TF即变换的英文单词TransForm的缩写。所以ROS和ROS2中的TF就是指和坐标变换相关的工具。在搞机器人当中,坐标变换经常用到,所以ROS2帮我们做了一个强大易用的TF工具。

生活不止眼前的苟且 6340

从匿名飞控到PIXhawk 4:一个ROS2开发者的真实踩坑与迁移心得

本文分享了从匿名飞控迁移到PIXhawk 4的实战经验,详细解析了硬件接口兼容性、MAVROS2配置要点和ROS2节点适配等关键挑战。通过对比PIXhawk 4的MAVLink2协议优势和匿名飞控的局限性,为无人机开发者提供了迁移路径和性能优化建议,显著提升开发效率和系统稳定性。

weixin_30415113的博客 437

【ROS2】初见ROS2中的TF2变换

在ROS 2中,TF2(Transform Library)是一个用于管理和转换坐标系的库。TF2可以帮助机器人应用程序在多个坐标系之间进行变换和同步,从而确保传感器数据和运动控制指令在正确的坐标系下进行操作。

MrZhang的博客 5676

ROS中基于Eigen与tf的IMU坐标系转换实战:从理论到代码实现

本文详细介绍了在ROS中使用Eigen与tf进行IMU坐标系转换的实战方法,从理论到代码实现全面解析。通过对比tf工具和Eigen库的优缺点,帮助开发者根据项目需求选择合适方案,实现IMU数据从机体坐标系到世界坐标系的精确转换,提升机器人开发效率。

weixin_29057237的博客 328

别再混淆了!用ROS2和LIO-SAM实战搞懂SLAM中的map、odom、base_link坐标系

本文通过ROS2和LIO-SAM实战,深入解析SLAM中map、odom、base_link坐标系的关系与应用。文章详细介绍了坐标系的角色定位、动态关系可视化方法,以及LIO-SAM的配置与调试技巧,帮助开发者解决机器人导航中的常见问题,提升定位精度。

weixin_33719619的博客 590

ROS2学习笔记2

本篇文章属于ROS2humble的学习笔记,来源于B站up主。下面是这位up主的视频链接。本文为个人学习笔记,只能做参考,细节方面建议观看视频,肯定受益匪浅。

2302_80867398的博客 461

[ROS] 小乌龟跟随--学习笔记

[ROS]初学者:小乌龟跟随的实现与部分代码解释

m0_56260823的博客 1392

ROS 2从入门到精通系列(十二):坐标变换(TF2)- 三维空间几何关系

ROS 2的TF2坐标变换系统详解:管理三维空间几何关系 摘要:本文全面介绍ROS 2中的TF2坐标变换系统,用于管理机器人系统中多个坐标系之间的空间关系。主要内容包括: 坐标变换基础概念:阐述3D坐标变换的数学表示(旋转矩阵和平移向量)以及四种旋转表示方法(欧拉角、四元数、旋转矩阵、轴角)的优缺点。 TF树结构:说明坐标系如何形成树形层级关系,并给出变换链相乘的数学模型,实现点在不同坐标系间的转换。 TF2核心组件: TransformBroadcaster:定期发布坐标系间变换关系

weixin_52694742的博客 1451

保姆级教程:在ROS2 Humble上搞定Livox Mid-360雷达,从IP配置到Rviz点云显示

本文提供在ROS2 Humble上配置Livox Mid-360激光雷达的完整教程,涵盖从IP设置到Rviz点云显示的全流程。详细解析硬件连接、驱动编译、配置文件优化及常见问题排查,帮助开发者快速搭建开发环境并实现高效点云可视化。

weixin_30955617的博客 184

ROS机器人开发实战:用tf库搞定四元数、欧拉角、旋转矩阵的6种转换(附C++/Python代码)

本文详细介绍了在ROS机器人开发中使用tf库实现四元数、欧拉角和旋转矩阵之间的6种高效转换方法,并提供了C++和Python代码示例。通过实战案例和调试技巧,帮助开发者解决姿态表示中的常见问题,提升机器人开发的效率和准确性。

weixin_42519985的博客 152

ROS Noetic下TF2静态坐标变换避坑指南:从`geometry_msgs`消息到Rviz可视化

本文详细解析了ROS Noetic下TF2静态坐标变换的实现与避坑技巧,涵盖从`geometry_msgs`消息处理到Rviz可视化的全流程。通过实战代码示例和常见问题排查表,帮助开发者解决依赖配置、坐标转换失败等典型问题,提升机器人系统开发效率。

weixin_30952535的博客 332

ROS2 - tf2

在本教程中,您将了解静态变换如何用于定义坐标系之间的静态关系,例如神秘海龟与世界坐标系的关系。此外,您还了解了静态变换如何通过将数据与通用坐标系相关联来帮助理解传感器数据(如激光扫描仪的数据)。最后,你们编写了自己的节点来向 tf2 发布静态变换,并学习了如何使用 static_transform_publisher 可执行文件和启动文件来发布所需的静态变换。在之前的教程中,我们创建了一个 tf2 广播器,将乌龟的姿势发布到 tf2。在本教程中,我们将创建一个 tf2 监听器,开始使用 tf2。

机器人控制与仿真应用 2609

ROS2中joint_states与TF协同原理及实操指南

在ROS2机器人系统中,joint_states话题承载关节状态数据,TF(Transform)则描述各坐标系间的空间关系,二者共同构成机器人运动学可视化与控制的基础。其核心原理是通过robot_state_publisher节点解析URDF模型,将实时joint_states映射为world至各link的动态坐标变换,实现‘关节能动、模型能显’。该机制支撑RViz2实时渲染、MoveIt2运动规划、Nav2自主导航等关键应用。掌握joint_states发布规范、TF树构建逻辑及常见QoS/时间戳/URD

weixin_29041767的博客 448

基于ROS2的四轮差速机器人运动控制与自主导航仿真全流程解析

移动机器人自主导航依赖感知、定位与规划的闭环协同,其中差速驱动模型是室内机器人底盘的主流构型。在ROS2与Gazebo仿真环境中,通过URDF构建精确物理模型,配置激光雷达与IMU传感器,结合slam_toolbox建图和Nav2导航框架,可实现从零搭建四轮差速机器人完整仿真系统。这种虚实结合的方法不仅降低硬件调试成本,还能提前验证运动学、传感器噪声和路径规划算法,为真机部署提供可靠基础。本文面向工程实践,梳理从环境搭建、模型配置到自主导航落地全流程,并给出常见问题的排查思路,帮助开发者快速构建可复现的移动

weixin_29165465的博客 235

MOS管关断时尖峰电压的5种实用抑制方案(附实测数据对比)

本文针对MOS管关断时产生的尖峰电压问题,提供了五种经过实验室实测验证的实用抑制方案。文章从尖峰电压的成因分析入手,详细阐述了优化PCB布局、设计RC缓冲电路、选用TVS二极管钳位、调节驱动电路以及使用磁珠吸收高频振荡等方法的原理、实施步骤与实测数据对比,为硬件工程师解决开关电源和电机驱动中的过压应力与EMI问题提供了系统性的实战指南。

1247

《动手学ROS2》6.4RVIZ2

本系列教程作者:小鱼 公众号:鱼香ROS QQ交流群:139707339 教学视频地址:小鱼的B站 完整文档地址:鱼香ROS官网 版权声明:如非允许禁止转载与商业用途。 6.4 RVIZ2 本节我们来说一说,机器人开发中常用的数据可视化工具RVIZ2。 1.RVIZ2是什么 RVIZ2是ROS2中的一个非常重要且常用的数据可视化工具。 那数据指的是什么数据?可视化又是怎么可视化的?RVIZ2又是如何实现不同数据的可视化的呢? 答案如下: 数据:各种调试机器人时常用的数据,比如:图像数据、三维点云数据.

生活不止眼前的苟且 3565

从VoLTE高清通话到5G新通话:手把手带你用Wireshark抓包分析QCI 1, 2, 5, 9的建立与切换过程

本文详细解析了从VoLTE高清通话到5G新通话的QCI(QoS Class Identifier)建立与切换过程,通过Wireshark抓包实战演示如何分析QCI 1、2、5、9的承载机制。内容涵盖QCI基础概念、实验环境搭建、VoLTE通话建立过程分析以及5G新通话中的QoS演进,为网络工程师和通信技术爱好者提供实用的技术指南和优化建议。

weixin_30788619的博客 343

【ROS入门】12. ROS中tf坐标系广播与监听的编程实现

本文介绍了在ROS中实现海龟跟随运动的tf广播器和监听器开发过程。首先创建工作空间和功能包,然后实现tf广播器程序,通过订阅海龟位姿信息广播其与世界坐标系的变换关系。接着实现tf监听器程序,获取两只海龟之间的坐标变换关系并发布跟随控制指令。最后配置编译规则并运行程序,通过键盘控制海龟1运动,海龟2将自动跟随。整个流程涵盖了ROS中tf变换的基本使用方法,实现了坐标系间的监听与广播功能。

纽约恋情的博客 218

vietjetair 航空的webpack多参数生成学习

工作内不是特别多,居安思危,有一个抓机票信息的公司面试前竟然需要做面试题,这就是一个题。看了下,有点麻烦,webpack 最初不得要领,看了下资料,找了个框架,练了两个简单的,搞这个吧。入库需要自己找,有几个参数不是明文的,算法不是很复杂,位置不太好找。整体难度一般。

liu_yueyang的博客 1511

java爬虫(爬取银行理财数据)

import java.io.IOException; import java.text.ParseException; import java.text.SimpleDateFormat; import java.util.ArrayList; import java.util.Calendar; import java.util.Date; import java.util.List; imp

ROCK 2491

【ROS2】一文看懂 Node、Topic、Service 与 TF(附小海龟实战比喻)

本文通过小海龟仿真器(Turtlesim)生动解释了ROS2的三大核心概念: 节点(Node)与话题(Topic):节点如同独立器官,话题是单向广播频道,实现异步通信; 服务(Service):类似办事窗口,支持双向同步请求-响应交互; 坐标变换(TF):作为"空间翻译官",管理多坐标系间的实时转换关系。 文中用"发传单"比喻话题通信,"打电话"比喻服务调用,并强调TF是实现机器人导航等复杂功能的基础。掌握这三点即可理解ROS2的核心通信机制。

qq_72758246的博客 250

基于Python的CT骨料图像分割与STL批量导出完整指南

本文介绍了一套完整的Python解决方案,用于CT骨料图像的分割与STL导出。针对CT图像中颗粒粘连、噪声伪影等技术难点,提出基于分水岭算法的分割策略,结合距离变换与标记控制实现颗粒分离。内容包括环境配置、数据读取与预处理、Otsu阈值分割、分水岭算法实现、表面提取与STL导出等关键步骤。教程提供详细代码及解释,帮助读者从CT体数据出发,最终生成每个颗粒的三维网格文件,便于后续定量分析和数值模拟。

huanghm88的专栏 240
上一篇: ROS2新版本有哪些改进
下一篇: 我的ROS2开发环境
首飞爱玩机器人
博客等级 码龄14年 6990粉丝 120原创
评论
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付:10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

首飞爱玩机器人

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值