ros2 tf坐标变换

坐标变换是机器人学中一个非常重要的概念，机器人本体及工作环境往往存在大量的元素，在机器人设计与机器人应用中涉及到不同组件的位置和姿态，这里引入坐标系及坐标变换概念。
坐标变换是机器人系统中非常基础的功能，ros2中由tf2功能包提供。
tf2是一个随时间跟踪多个坐标系的功能包，树形结构故不支持并联机器人。
使用tf2功能包总的来说就只有两步：
1、监听TF变换2、广播TF变换
通常我们可以使用tf2_ros功能包的tf2_echo节点监听tf变换

ros2 run tf2_ros tf2_monitor foo bar

当然，该功能包还有static_transform_publisher节点可以发布静态tf变换

ros2 run tf2_ros static_transform_publisher 0 0 1 0 0 0 1 foo bar

那么在程序中该怎么使用tf监听与广播呢，下面通过小乌龟跟随来体验tf监听器与广播器
首先进入工作空间创建tutorials_tf功能包

ln@ln-pctogo:~/ros2_ws$ ros2 pkg create tutorials_tf --build-type ament_cmake --dependencies rclcpp std_msgs tf2 tf2_ros turtlesim geometry_msgs

然后进入src文件夹创建广播器节点文件turtle_tf_broadcaster.cpp，代码如下：

#include "turtlesim/msg/pose.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"

std::string turtle_name;
//节点句柄
rclcpp::Node::SharedPtr node_handle = nullptr;

void pose_callback(const turtlesim::msg::Pose::SharedPtr pose)
{
    //tf广播器
    static tf2_ros::TransformBroadcaster pose_broadcaster_(node_handle);
    //广播器消息类型实例化
    geometry_msgs::msg::TransformStamped pose_tf;
    //根据海龟当前位姿，发布对世界坐标系的变换
    pose_tf.header.stamp = node_handle->now();
    pose_tf.header.frame_id = "world";
    pose_tf.child_frame_id = turtle_name;
    pose_tf.transform.translation.x = pose->x;
    pose_tf.transform.translation.y = pose->y;
    pose_tf.transform.translation.z = 0.0;
    //yaw转四元数
    tf2::Quaternion quaternion;
    // yaw, pitch and roll are rotations in z, y, x respectively
    quaternion.setRPY(0,0,pose->theta);
    pose_tf.transform.rotation = tf2::toMsg(quaternion);
    //发布坐标变换
    pose_broadcaster_.sendTransform(pose_tf);
}

int main(int argc, char** argv)
{
	rclcpp::init(argc, argv);
    //初始化节点
    node_handle = rclcpp::Node::make_shared("turtle_tf_broadcaster");
    //获得小海龟的名字
    if (argc != 2)
    {
        RCLCPP_ERROR(node_handle->get_logger(), "error Exiting");
        return -1;
    };
    turtle_name = argv[1];
    //接收小海龟的位姿信息
    auto subscription = node_handle->create_subscription<turtlesim::msg::Pose>(turtle_name+"/pose", 10, pose_callback); 
    //开始接收topic
    rclcpp::spin(node_handle);
    rclcpp::shutdown();
    return 0;
}

然后创建一个监听器，turtle_tf_listener.cpp代码如下：

#include "turtlesim/srv/spawn.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/msg/twist.hpp"

#define _USE_MATH_DEFINES
//tf监听器类（tf2_echo）
class echoListener
{
public:
    tf2_ros::Buffer buffer_;
    std::shared_ptr<tf2_ros::TransformListener> tfl_;
    //constructor with name
    echoListener(rclcpp::Clock::SharedPtr clock) :
    buffer_(clock)
    {
    tfl_ = std::make_shared<tf2_ros::TransformListener>(buffer_);
    };

    ~echoListener(){};
private:
};

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    auto node_handle = rclcpp::Node::make_shared("turtle_tf_listener");
    //创建“add_two_ints”客户端
    auto client = node_handle->create_client<turtlesim::srv::Spawn>("spawn");
    //等待连接服务器
    while (!client->wait_for_service(std::chrono::seconds(1))) {
    //关闭节点
    if (!rclcpp::ok()) {
        RCLCPP_ERROR(node_handle->get_logger(), "Interrupted while waiting for the service. Exiting");
        return 0;
    }
        RCLCPP_INFO(node_handle->get_logger(), "service not available, waiting again...");
    }
    //实例化消息
    auto request = std::make_shared<turtlesim::srv::Spawn::Request>();
    //生成小乌龟
    auto result_future = client->async_send_request(request);
    //乌龟速度发布者
    auto vel_pub = node_handle->create_publisher<geometry_msgs::msg::Twist>("turtle2/cmd_vel", 10);
    //延时 10HZ
    rclcpp::WallRate loop_rate(10.0);
    //时间
    rclcpp::Clock::SharedPtr clock = node_handle->get_clock();
    //实例化监听器echoListener
    echoListener echoListener(clock);
    //监听"turtle2"->"turtle1"
    echoListener.buffer_.canTransform("turtle2", "turtle1", tf2::TimePoint(), tf2::durationFromSec(1.0));
    //开始循环
    while(rclcpp::ok()) {
        //监听两只小海龟的坐标变换
        geometry_msgs::msg::TransformStamped echo_transform;
        echo_transform = echoListener.buffer_.lookupTransform("turtle2", "turtle1", tf2::TimePoint());
        auto translation = echo_transform.transform.translation;
        //根据两只熊海龟之间的坐标关系计算出龟2的线速度与角速度，幷将其发布出去。
        geometry_msgs::msg::Twist vel;
        vel.angular.z = 4.0 * atan2(translation.y,
                                        translation.x);
        vel.linear.x = 0.5 * sqrt(pow(translation.x, 2) +
                                      pow(translation.y, 2));
        vel_pub->publish(vel);
        //延时
        loop_rate.sleep();
    }
    rclcpp::shutdown();
    return 0;
}

然后在功能包节点CMakeLists.txt添加编译规则生成节点：

add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
ament_target_dependencies(turtle_tf_broadcaster "rclcpp" "tf2" "tf2_ros" "turtlesim")
add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
ament_target_dependencies(turtle_tf_listener "rclcpp" "tf2" "tf2_ros" "turtlesim" "geometry_msgs")

target_link_libraries(
  turtle_tf_broadcaster
  ${catkin_LIBRARIES})

  install(TARGETS
    turtle_tf_broadcaster
    turtle_tf_listener
    EXPORT export_${PROJECT_NAME}
    DESTINATION lib/${PROJECT_NAME})

同样的colcon build编译幷source一下。
现在你可以启动了
首先打开小海龟节点：

ln@ln-pctogo:~$ ros2 run turtlesim turtlesim_node 

然后开启龟1的tf广播器：

ln@ln-pctogo:~$ ros2 run tutorials_tf turtle_tf_broadcaster turtle1

接着开启龟2的tf广播器：

ln@ln-pctogo:~$ ros2 run tutorials_tf turtle_tf_broadcaster turtle2

打开监听器：

ln@ln-pctogo:~$ ros2 run tutorials_tf turtle_tf_listener 

你会看到一只变态海龟从左下角出生幷尾随至窗口中间的小乌龟，现在你可以运行速度发布节点控制龟1逃离：

ln@ln-pctogo:~$ ros2 run turtlesim turtle_teleop_key 

我们可以看到后面的乌龟穷追不舍，ros2的部分小海龟是带着锤子的，比如在程序调试过程中左下角产生的海龟尸体。