-The TF Subsystem

Posted 2023-03-01 zhangrelay

这个是ROS2机器人的核心工具之一。

概念：

TF（坐标系转换）子系统是ROS2机器人框架中的一个重要组件，它的功能是提供坐标系转换服务，使得不同坐标系之间的数据可以转换。比如，机器人的传感器可以产生的数据是基于机器人本体坐标系的，而机器人的末端机械臂可以根据这些数据来控制机械臂的运动，但是机械臂需要的数据是基于机械臂坐标系的，这时候TF子系统就可以把数据从机器人本体坐标系转换成机械臂坐标系，从而实现机械臂的控制。

应用：

ROS2机器人TF子系统是一种基于消息传递的机器人框架，它可以帮助机器人开发者和研究人员更轻松、更快速地开发机器人系统。TF子系统可以帮助机器人开发者实现机器人定位、导航和路径规划等功能。比如，一个机器人可以使用TF子系统来实现室内定位，可以使用TF子系统来实现自主导航，也可以使用TF子系统来实现复杂的路径规划。

坐标变换消息：

ros2 interface show tf2_msgs/msg/TFMessage

geometry_msgs/TransformStamped[] transforms

例如机器人turtlebot、tiago等。

使用：

ros2 run rqt_tf_tree rqt_tf_tree

如果是类人机器人：

注意，如果没有指令，需要安装rqt-tf-tree功能包。

husky

为何选择这个案例呢？

机器人编程基础-ETH ROS 2022-Programming for Robotics_zhangrelay的博客-CSDN博客

2021（noetic）：

机器人编程基础-ETH ROS

2020及之前（indigo/kinetic/melodic）：

ROS编程基础课程2020更新资料和习题解答说明

---

书中所有案例均可以用husky和turtlebot3等复现。

典型案例：

odom →base footprint→ detected obstacle

其中base footprint→ detected obstacle：

geometry_msgs::msg::TransformStamped detection_tf;
detection_tf.header.frame_id = "base_footprint";
detection_tf.header.stamp = now();
detection_tf.child_frame_id = "detected_obstacle";
detection_tf.transform.translation.x = 1.0;
tf_broadcaster_->sendTransform(detection_tf);

odom →base footprint→ detected obstacle：

tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer);
...
geometry_msgs::msg::TransformStamped odom2obstacle;
odom2obstacle = tfBuffer_.lookupTransform("odom", "detected_obstacle", tf2::TimePointZero);

ROS2机器人的tf2::TimePointZero是一个用来表示时间的类，它使用“秒”和“纳秒”来表示时间，可以用来跟踪和比较时间点。它可以用来构建时间戳，并且可以与其他时间类型（例如ros::Time）进行转换。

C++案例：

#include <chrono>
#include <functional>
#include <memory>

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_broadcaster.h"

using namespace std::chrono_literals;

class FixedFrameBroadcaster : public rclcpp::Node

public:
  FixedFrameBroadcaster()
  : Node("fixed_frame_tf2_broadcaster")
  
    tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
    timer_ = this->create_wall_timer(
      100ms, std::bind(&FixedFrameBroadcaster::broadcast_timer_callback, this));
  

private:
  void broadcast_timer_callback()
  
    geometry_msgs::msg::TransformStamped t;

    t.header.stamp = this->get_clock()->now();
    t.header.frame_id = "turtle1";
    t.child_frame_id = "carrot1";
    t.transform.translation.x = 0.0;
    t.transform.translation.y = 2.0;
    t.transform.translation.z = 0.0;
    t.transform.rotation.x = 0.0;
    t.transform.rotation.y = 0.0;
    t.transform.rotation.z = 0.0;
    t.transform.rotation.w = 1.0;

    tf_broadcaster_->sendTransform(t);
  

rclcpp::TimerBase::SharedPtr timer_;
  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
;

int main(int argc, char * argv[])

  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<FixedFrameBroadcaster>());
  rclcpp::shutdown();
  return 0;

发布：

#include <functional>
#include <memory>
#include <sstream>
#include <string>

#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"

class FramePublisher : public rclcpp::Node

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

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

    // Subscribe to a turtle12/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, std::placeholders::_1));
  

private:
  void handle_turtle_pose(const std::shared_ptr<turtlesim::msg::Pose> msg)
  
    geometry_msgs::msg::TransformStamped t;

    // Read message content and assign it to
    // corresponding tf variables
    t.header.stamp = this->get_clock()->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;

订阅：

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

#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"

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
    target_frame_ = this->declare_parameter<std::string>("target_frame", "turtle1");

    tf_buffer_ =
      std::make_unique<tf2_ros::Buffer>(this->get_clock());
    tf_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 t;

        // Look up for the transformation between target_frame and turtle2 frames
        // and send velocity commands for turtle2 to reach target_frame
        try 
          t = tf_buffer_->lookupTransform(
            toFrameRel, fromFrameRel,
            tf2::TimePointZero);
         catch (const 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(
          t.transform.translation.y,
          t.transform.translation.x);

        static const double scaleForwardSpeed = 0.5;
        msg.linear.x = scaleForwardSpeed * sqrt(
          pow(t.transform.translation.x, 2) +
          pow(t.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> tf_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;

时间：

transformStamped = tf_buffer_->lookupTransform(
   toFrameRel,
   fromFrameRel,
   tf2::TimePointZero);

时间旅行：

rclcpp::Time when = this->get_clock()->now() - rclcpp::Duration(5, 0);
t = tf_buffer_->lookupTransform(
    toFrameRel,
    fromFrameRel,
    when,
    50ms);