前言
本篇文章属于ROS2humble的学习笔记，来源于B站鱼香ROSup主。下面是这位up主的视频链接。本文为个人学习笔记，只能做参考，细节方面建议观看视频，肯定受益匪浅。
《ROS 2机器人开发从入门到实践》课程介绍_哔哩哔哩_bilibili
一、坐标变换工具TF
1. Python中的手眼变换坐标变换
（1）静态TF
import rclpy
from rclpy.node import Node
from tf2_ros import StaticTransformBroadcaster # 静态坐标发布器
from geometry_msgs.msg import TransformStamped # 消息接口
from tf_transformations import quaternion_from_euler # 欧拉角转四元数
import math # 角度转弧度函数class StaticTFBroadcaster(Node):def __init__(self):super().__init__('static_tf_broadcaster')self.static_broadcaster_ = StaticTransformBroadcaster(self)self.publish_static_tf()def publish_static_tf(self):"""发布静态TF 从 base_link 到 camera_link 之间的坐标关系"""transform = TransformStamped()transform.header.frame_id = 'base_link'transform.child_frame_id = 'camera_link'transform.header.stamp = self.get_clock().now().to_msg()transform.transform.translation.x = 0.5transform.transform.translation.y = 0.3transform.transform.translation.z = 0.6q = quaternion_from_euler(math.radians(180),0,0)transform.transform.rotation.x = q[0]transform.transform.rotation.y = q[1]transform.transform.rotation.z = q[2]transform.transform.rotation.w = q[3]self.static_broadcaster_.sendTransform(transform)self.get_logger().info(f'发布静态TF:{transform}')def main():rclpy.init()node = StaticTFBroadcaster()rclpy.spin(node)rclpy.shutdown()
（2）动态TF
import rclpy
from rclpy.node import Node
from tf2_ros import TransformBroadcaster # 坐标发布器
from geometry_msgs.msg import TransformStamped # 消息接口
from tf_transformations import quaternion_from_euler # 欧拉角转四元数
import math # 角度转弧度函数class TFBroadcaster(Node):def __init__(self):super().__init__('tf_broadcaster')self.broadcaster_ = TransformBroadcaster(self)self.timer_ = self.create_timer(0.01,self.publish_tf)def publish_tf(self):"""发布动态TF 从 camera_link 到 bottle_link 之间的坐标关系""" transform = TransformStamped()transform.header.frame_id = 'camera_link'transform.child_frame_id = 'bottle_link'transform.header.stamp = self.get_clock().now().to_msg()transform.transform.translation.x = 0.2transform.transform.translation.y = 0.3transform.transform.translation.z = 0.5q = quaternion_from_euler(0,0,0)transform.transform.rotation.x = q[0]transform.transform.rotation.y = q[1]transform.transform.rotation.z = q[2]transform.transform.rotation.w = q[3]self.broadcaster_.sendTransform(transform)self.get_logger().info(f'发布静态TF:{transform}')def main():rclpy.init()node = TFBroadcaster()rclpy.spin(node)rclpy.shutdown()（3）查询TF关系
import rclpy
from rclpy.node import Node
from tf2_ros import TransformListener,Buffer # 坐标监听器
from tf_transformations import euler_from_quaternion # 四元数转欧拉角
import math # 角度转弧度函数
import rclpy.timeclass TFListener(Node):def __init__(self):super().__init__('tf_listener')self.buffer_ = Buffer()self.listener_ = TransformListener(self.buffer_,self)self.timer_ = self.create_timer(1.0,self.get_transform)def get_transform(self):"""实时查询坐标关系""" try:result = self.buffer_.lookup_transform('base_link','bottle_link',rclpy.time.Time(seconds=0.0),rclpy.time.Duration(seconds=1.0))transform = result.transformself.get_logger().info(f'平移:{transform.translation}')self.get_logger().info(f'旋转:{transform.rotation}')rotation_euler = euler_from_quaternion([transform.rotation.x,transform.rotation.y,transform.rotation.z,transform.rotation.w,])self.get_logger().info(f'旋转RPY:{rotation_euler}')except Exception as e:self.get_logger().warn(f'获取坐标变换失败,原因:{str(e)}')def main():rclpy.init()node = TFListener()rclpy.spin(node)rclpy.shutdown()2. C++中的地图坐标变换
（1）静态TF
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/static_transform_broadcaster.h"class StaticTFBroadcaster:public rclcpp::Node
{
private:std::shared_ptr<tf2_ros::StaticTransformBroadcaster> broadcaster_;
public:StaticTFBroadcaster() : Node("static_tf_broadcaster"){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();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 q;q.setRPY(0.0,0.0,60*M_PI/180.0);transform.transform.rotation = tf2::toMsg(q);this->broadcaster_->sendTransform(transform);}
};int main(int argc,char* argv[])
{rclcpp::init(argc,argv);auto node = std::make_shared<StaticTFBroadcaster>();rclcpp::spin(node);rclcpp::shutdown();
}（2）动态TF
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/transform_broadcaster.h"
#include "chrono"
using namespace std::chrono_literals;class TFBroadcaster:public rclcpp::Node
{
private:std::shared_ptr<tf2_ros::TransformBroadcaster> broadcaster_;rclcpp::TimerBase::SharedPtr timer_;public:TFBroadcaster() : Node("tf_broadcaster"){broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);// this->publish_tf();timer_ = this->create_wall_timer(100ms,std::bind(&TFBroadcaster::publish_tf,this));}void publish_tf(){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 q;q.setRPY(0.0,0.0,30*M_PI/180.0);transform.transform.rotation = tf2::toMsg(q);this->broadcaster_->sendTransform(transform);}
};int main(int argc,char* argv[])
{rclcpp::init(argc,argv);auto node = std::make_shared<TFBroadcaster>();rclcpp::spin(node);rclcpp::shutdown();
}（3）查询TF关系
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "tf2/utils.h"
#include "chrono"
using namespace std::chrono_literals;class TFListener:public rclcpp::Node
{
private:std::shared_ptr<tf2_ros::TransformListener> listener_;std::shared_ptr<tf2_ros::Buffer> buffer_;rclcpp::TimerBase::SharedPtr timer_;public:TFListener() : Node("tf_listener"){this->buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());this->listener_ = std::make_shared<tf2_ros::TransformListener>(*buffer_,this);// this->publish_tf();timer_ = this->create_wall_timer(1s,std::bind(&TFListener::get_tf,this));}void get_tf(){// 到Buffer查询坐标关系try{const auto transform = buffer_->lookupTransform("base_link","target_point",this->get_clock()->now(),rclcpp::Duration::from_seconds(1.0f));auto translation = transform.transform.translation;auto rotation = transform.transform.rotation;double y,p,r;tf2::getEulerYPR(rotation,y,p,r);RCLCPP_INFO(get_logger(),"平移:%f,%f,%f",translation.x,translation.y,translation.z);RCLCPP_INFO(get_logger(),"旋转:%f,%f,%f",y,p,r);}catch(const std::exception& e){RCLCPP_WARN(get_logger(),"%s",e.what());}}// void publish_tf()// {// 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 q;// q.setRPY(0.0,0.0,30*M_PI/180.0);// transform.transform.rotation = tf2::toMsg(q);// this->broadcaster_->sendTransform(transform);// }
};int main(int argc,char* argv[])
{rclcpp::init(argc,argv);auto node = std::make_shared<TFListener>();rclcpp::spin(node);rclcpp::shutdown();
}
