【TF】

【TF】

坐标关系

ROS2可视化
基于话题通信

【例】 对于不同坐标系 某物有多远
base_link机器人基坐标
base_laser雷达坐标系

命令行使用TF

发布静态坐标 static_transform_publisher

※rotation用四元数表示
发布坐标变换 tf2_echo

ros2 run tf2_ros tf2_echo base_link wall_point

利用工具查看所有坐标系连接关系

ros2 run tf2_tools view_frames

TF原理

发布动态TF：数据被发布到/tf话题上
查询坐标关系变化：订阅/tf和/tf_static话题->查询数据

【例】地图坐标变换

发布静态TF

依赖

消息接口依赖 geometry_msgs
提供消息类型的转换函数 tf2_geometry_msgs

代码

#include<memory>
#include"geometry_msgs/msg/transform_stamped.hpp"
#include"rclcpp/rclcpp.hpp"
#include"tf2/LinearMath/Quaternion.h"
#include"tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include"tf2_ros/static_transform_broadcaster.h"

class StaticTFBroadcastrer : public rclcpp::Node{
    public:
	//在构造函数中进行
        StaticTFBroadcastrer() : Node("tf_broadcaster_node"){
            //创建静态广播发布器并发布
            broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);//初始化
            this->publish_tf();//进行发布
        }

        void publish_tf(){
	    //构建消息接口对象
            geometry_msgs::msg::TransformStamped transform;
            transform.header.stamp=this->get_clock()->now();//时间戳赋值1
            transform.header.frame_id = "map";//父节点
            transform.child_frame_id = "target_point";//子结点
            transform.transform.translation.x = 5.0;
            transform.transform.translation.y = 3.0;
            transform.transform.translation.z = 0.0;
            tf2::Quaternion quat;
            quat.setRPY(0,0,60*M_PI/180);//弧度值欧拉角转四元数
            transform.transform.rotation = tf2::toMsg(quat);//转成消息接口类型
            broadcaster_->sendTransform(transform);//将坐标变换发布出去(静态变换只需要发布一次)
        }

    private:
        std::shared_ptr<tf2_ros::StaticTransformBroadcaster> broadcaster_;
};

int main(int argc,char** argv){
    rclcpp::init(argc,argv);
    auto node=std::make_shared<StaticTFBroadcastrer>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

CMakeLists.txt添加依赖

ament_target_dependencies(static_tf_broadcaster rclcpp tf2 tf2_ros geometry_msgs tf2_geometry_msgs)

发布动态TF

不断向外发布->timer定时器包装定时发布

代码

#include<memory>
#include"geometry_msgs/msg/transform_stamped.hpp" //提供消息接口
#include"rclcpp/rclcpp.hpp"
#include"tf2/LinearMath/Quaternion.h"             //提供tf2::Quaternion类
#include"tf2_geometry_msgs/tf2_geometry_msgs.hpp" //提供消息类型转换函数
#include"tf2_ros/transform_broadcaster.h"         //提供坐标广播器类:动态广播
#include<chrono>
using namespace std::chrono_literals;

class DynamicTFBroadcaster : public rclcpp::Node{
    public:
        DynamicTFBroadcaster() : Node("dynamic_tf_broadcaster"){
            tf_broadcaster_=std::make_shared<tf2_ros::TransformBroadcaster>(this);
            //定时器
            timer_=create_wall_timer(10ms,std::bind(&DynamicTFBroadcaster::publishTransform,this));
        }

        void publishTransform(){
            geometry_msgs::msg::TransformStamped transform;
            transform.header.stamp=this->get_clock()->now();
            transform.header.frame_id = "map";
            transform.child_frame_id = "base_link";
            transform.transform.translation.x=2.0;
            transform.transform.translation.y=3.0;
            transform.transform.translation.z=0.0;
            tf2::Quaternion quat;
            quat.setRPY(0,0,30*M_PI/180);
            transform.transform.rotation = tf2::toMsg(quat);
            tf_broadcaster_->sendTransform(transform);
        }

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

int main(int argc,char** argv){
    rclcpp::init(argc,argv);
    auto node=std::make_shared<DynamicTFBroadcaster>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

查询TF关系

#include<memory>//智能指针相关
#include"geometry_msgs/msg/transform_stamped.hpp"//提供消息接口
#include"rclcpp/rclcpp.hpp"
#include"tf2/LinearMath/Quaternion.h"
#include"tf2/utils.h"//提供tf2:getEulerYPR函数->将四元数转换为欧拉角
#include"tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include"tf2_ros/buffer.h"//提供TF缓冲类Buffer->存储订阅到的TF数据
#include"tf2_ros/transform_listener.h"//提供坐标监听器类->订阅TF变换
#include<chrono>
using namespace std::chrono_literals;

class TFListener : public rclcpp::Node{
    public:
        TFListener() : Node("tf_listener"){
            buffer_=std::make_shared<tf2_ros::Buffer>(this->get_clock());
            listener_=std::make_shared<tf2_ros::TransformListener>(*buffer_,this);
                                                                //*buffer_:获取指针指向的原始对象再传入
            timer_=this->create_wall_timer(5s,std::bind(&TFListener::getTransform,this));
        }
        void getTransform(){
            //使用try-catch对异常进行捕获
            try{
                //查询坐标变换
                const auto transform = buffer_->lookupTransform(
                    "base_link","target_point",this->get_clock()->now(),//数据开始时间
                    rclcpp::Duration::from_seconds(1.0f));//超时时间
                //转换结果并输出
                const auto &translation = transform.transform.translation;
                const auto &rotation = transform.transform.rotation;
                double yaw,pitch,roll;
                tf2::getEulerYPR(rotation,yaw,pitch,roll);//四元数转欧拉角
                RCLCPP_INFO(get_logger(),"平移分量：（%f,%f,%f)",translation.x,translation.y,translation.z);
                RCLCPP_INFO(get_logger(),"旋转分量：（%f,%f,%f)",roll,pitch,yaw);
            }
            catch(tf2::TransformException &ex){//处理异常
                RCLCPP_WARN(get_logger(),"异常：%s",ex.what());
            }
        }
    
    private:
        std::shared_ptr<tf2_ros::Buffer> buffer_;
        std::shared_ptr<tf2_ros::TransformListener> listener_;
        rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc,char** argv){
    rclcpp::init(argc,argv);
    auto node=std::make_shared<TFListener>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}