Ros2_学习整理_坐标系变换_11（赵虚左老师）

干掉乔治的猪

已于 2024-06-07 10:01:12 修改

阅读量800

点赞数 16

分类专栏： Ros2理论与实践文章标签：学习 python 机器人 c++ ros2

于 2024-05-30 10:59:57 首次发布

本文链接：https://blog.csdn.net/m0_51759289/article/details/139318257

版权

Ros2理论与实践专栏收录该内容

12 篇文章 0 订阅

订阅专栏

坐标变换

1.坐标变换概述
tf(坐标变换)允许用户随着时间跟随多个坐标系，他在时间缓冲的树结构中维护坐标帧之间的关系。允许用户任意时间点、任意坐标帧之间变换点、向量等。
完整的坐标变换由坐标变换广播方和坐标变换监听方组成
广播方:
发布一组坐标系相对关系。
监听方：
将多组坐标系相对关系融合为一颗坐标树并实现任意坐标系之间或坐标系与坐标点之间的变换。
2.坐标系变换广播

2-1.坐标变换相对关系：

1、静态坐标系相对关系（两个坐标系之间的相对位置固定不变）
2、动态坐标系相对关系（两个坐标系之间的相对位置关系是动态改变的）

2-2.静态广播器（C++）

2-2-1.头文件

//静态坐标系变换所依赖的消息载体
#include "geometry_msgs/msg/Transform_stamped.hpp" 
//rclcpp
#include "rclcpp/rclcpp.hpp"
//将欧拉角转换成四元数所依赖的库
#include "tf2/LinearMath/Quaternion.h"
//静态坐标系广播所依赖的库
#include "tf2_ros/static_transform_broadcaster.h"

2-2-2.核心实现

// 4-1.创建静态坐标变换发布方；

std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_publisher_ 
tf_publisher_ =std::make_unique<tf2_ros::StaticTransformBroadcaster>() ;

// 4-2.组织并发布消息。

this->make_transforms(transformation);
def make_transforms(char * transformation[]){
	//组织消息
	geometry_msgs::msg::Transformstamped t;
	rclcpp::Time now = this->get_clock()->now();
	t.header.stamp = now;//时间戳
	t.header.fram_id = transformation[7];//父级坐标系
	t.child_frame_id = transformation[8];//子级坐标系
	//设置偏移量
	t.transform.translation.x = atof(transformation[1]);
	t.transform.translation.x = atof(transformation[2]);
	t.transform.translation.x = atof(transformation[3]);
	//设置四元数
	//将欧拉角转换成四元数
	tf2::Quaternion q;
	q.setRPY(
	 atof(transformation[4]),
	 atof(transformation[5]),
	 atof(transformation[6]));
	//设置旋转量
	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);

验证：

ros2 run cpp03_tf_broadcaster demo01_static_tf_broadcaster -0.5 0 0.4 0 0 0 base_link camera

2-3.静态广播器(Python)

2-3-1.导包

#系统库函数
import sys 
#Ros2端库函数
import rclpy  
#坐标转换所依赖的库函数
import tf_transformations  
#Node节点库函数
from rclpy.node import Node  
#静态广播所依赖的消息载体
from geometry_msgs.msg import TransformStamped 
#静态坐标变换广播所依赖的库函数
from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster

2-3-2.核心实现

#创建静态坐标变换发布方
self.tf_publisher_ = StaticTransformBroadcaster(self)
#组织并发布消息
self.make_transforms(transformation)
def make_transforms(self,transformation):
	#组织消息
	t= TransformStamped()
	t.header.stamped = self.get_clock().now().to_msg()#时间戳
	t.header.frame_id = sys.argv[7]#父级坐标系
	t.child_frame_id = sys.argv[8]#子级坐标系
	#设置偏移量
	t.transform.translation.x = flaot(sys.argv[1])
	t.transform.translation.y = flaot(sys.argv[2])
	t.transform.translation.z = flaot(sys.argv[3])
	#转换为四元数
	quat = tf_transformations.quaternion_from_euler(
	float(sys.argv[4]),float(sys.argv[5]),float(sys.argv[6]))
	设置旋转量
	t.transform.rotation.x = quat[0]
	t.transform.rotation.x = quat[1]
	t.transform.rotation.x = quat[2]
	t.transform.rotation.x = quat[3]
	#发布消息
	self.publisher_.sendTransform(t)

验证：

ros2 run py03_tf_broadcaster demo01_static_tf_broadcaster_py -0.5 0 0.4 0 0 0 base_link camera

2-4.动态广播器（C++）

2-4-1.头文件

//动态坐标系变换所依赖的消息载体
#include <geometry_msgs/msg/transform_stamped.hpp>
//ros2
#include <rclcpp/rclcpp.hpp>
//坐标系变换用到的数学计算库
#include <tf2/LinearMath/Quaternion.h>
//广播动态坐标系变换所依赖的库
#include <tf2_ros/transform_broadcaster.h>
//小乌龟的位置消息载体
#include <turtlesim/msg/pose.hpp>

2-4-2.核心实现

// 3-1.创建动态坐标变换发布方；
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
tf_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
//订阅乌龟的位姿
rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr sub_;
sub_ = this->create_subscription<turtlesim::msg::Pose>("/turtle1/pose",10,std::bind(&DynamicBroadcaster::handle_turtle_pose,this,std::placeholders::_1));
//将订阅到的乌龟位姿生成坐标帧并广播
void andle_turtle_pose(const turtlesim::msg::Pose &msg){
	geometry_msgs::msg::TransformStamped t;
	rclcpp::Time now = this->get_clock().now()
	t.header.stamp = now;  //时间戳
	t.header.frame_id = "world";  
	t.child_frame_id = "turtle1";
	//设置偏移量
	t.transform.translation.x = msg.x;
	t.transform.translation.y = msg.y;
	t.transform.translation.z = 0.0;
	//设置四元数
	tf2::Quaternion q;
	q.setRPY(0.0,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();
	//发送消息
	tf_broadcaster_->sendTransform(t);
}

测试：

ros2 run cpp03_tf_broadcaster demo02_dynamic_tf_broadcaster

2-5.动态广播器（Python）

2-5-1.导包

import rclpy
#数学计算所依赖的库
import tf_transformations
#Node依赖的库
from rclpy.node import Node
#所依赖的消息类型
from geometry_msgs.msg import TransformStamped
#坐标系转换动态广播
from tf2_ros import TransformBroadcaster
from turtlesim.msg import Pose

2-5-2.核心实现

# 3-1.创建动态坐标变换发布方；
self.tf_broadcaster_ = TransformBroadcaster()
# 3-2.创建乌龟位姿订阅方；
self.sub_ = self.create_subscripition(Pose,"/turtle1/pose",self.handle_turtle_pose,10)
def handle_turtle_pose(self,msg):
	#组织消息
	t = TransformStamped()
	t.header.stamp = self.get_clock().now().to_msg()
	t.header.frame_id = "world"
	t.child_frame_id = "turtle1"
	#设置偏移量
	t.transform.translation.x = msg.x
	t.transform.translation.y = msg.y
	t.transform,.translation.z = 0.0
	#转换四元数
	q = tf_transformations.quaternoin_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]
	//发送消息
	self.tf_broadcaster_.sendTransform(t)

测试：

ros2 run py03_tf_broadcaster demo02_dynamic_tf_broadcaster_py

3.坐标点变换广播

3-1.坐标点发布（C++）

3-1-1.头文件

#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/point_stamped.hpp"

3-1-2.核心实现

//3-1.创建坐标点发布方；
rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr tf_point_pub_;
tf_point_pub_ = this->create_publisher<geometry_msgs::msg::PointStamped>("point",10);
//创建定时器
rclcpp::TimerBase::SharedPtr  timer_;
timer_ = this->>>create_wall_timer(0.1,std::bind(&PointBroadcaster::on_timer,this));
//组织并发布消息
void on_timer(){
	geometry_msgs::msg::PointStamped point;
	point.header.stamp = this->get_clock().now();//this->now()
	point.header.frame_id = "laser";
	point.point.x = x;
	point.point.y = 0.0;
	point.point.z = 0.0;
	//发送消息
	tf_point_pub_->publish(point);
}

测试：

终端1：ros2 run cpp03_tf_broadcaster demo01_static_tf_broadcaster 0.4 0 0.2 0 0 0 base_link laser
终端2：ros2 run cpp03_tf_broadcaster demo03_point_publisher

3-2.坐标点发布（Python）

3-2-1.导包

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PointStamped

3-2-2.核心实现

#3-1.创建坐标点发布方；
self.tf_point_pub_ = self.create_publisher(PointStamped,"point",10)
#3-2.创建定时器；
self.timer_ = self.create_wall_timer(1.0,self.on_timer)
self.x = 0.01
#3-3.组织并发布坐标点消息
def on_timer():
	point_ = PointStamped()
	point_.header.stamp = self.get_clock().now().to_msg()
	point_.header.frame_id = "laser"
	self.x += 0.01
	point_.point.x = x
	point_.point.y = 0.2
point_.point.z = 0.3
//发送消息
self.tf_point_pub_.publish(point)

测试：

终端1：ros2 run py03_tf_broadcaster demo01_static_tf_broadcaster_py 0.4 0 0.2 0 0 0 base_link laser
终端2：ros2 run py03_tf_broadcaster demo03_point_publisher_py

4.坐标系变换监听
监听可以实现坐标点在不同坐标系之间的切换，或者不同坐标系之间的变换。

4-1.坐标系变换（C++）

4-1-1.头文件

#include "rclcpp/rclcpp.hpp"
//监听方所依赖的库
#include "tf2_ros/transform_listener.h"
//缓存所依赖的库
#include "tf2_ros/buffer.h"
//坐标变换数学计算所依赖的库
#include "tf2/LinearMath/Quaternion.h"

4-1-2.核心实现

//3-1.创建tf缓存对象指针；
std::unique_ptr<tf2_ros::Buffer> buffer_;
buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
//3-2.创建tf监听器；
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*this);
//创建定时器timer_
rclcpp::TimeBase::SharedPtr timer_;
timer_ = this->create_wall_timer(1.0,std::bind(&FrameListener::on_timer,this));
void on_timer(){
try{
	//3-3.按照条件查找符合条件的坐标系并生成变换后的坐标帧。
		/*
		API:lookup_transform()
		参数：
		1、希望变换到的坐标系
		2、你当前所在的坐标系，或者你想要从哪个坐标系开始变换
		3、希望获取变换关系的时间点。这通常是一个ros2::Time对象，表示一个特定的ROS 2时间戳。
		*/
	auto tfStamped = buffer_->lookupTransform("camera","laser",tf2::TimePointZero);
	//转换结果
	 RCLCPP_INFO(this->get_logger(),"frame_id:%s",transformStamped.header.frame_id.c_str());
	RCLCPP_INFO(this->get_logger(),"child_frame_id:%s",transformStamped.child_frame_id.c_str());
	RCLCPP_INFO(this->get_logger(),"坐标:(%.2f,%.2f,%.2f)", transformStamped.transform.translation.x,transformStamped.transform.translation.y,transformStamped.transform.translation.z);
	}
catch(const tf2::LookupException& e){
	 RCLCPP_INFO(this->get_logger(),"坐标变换异常：%s",e.what());
	}
}

4-2.坐标系变换（python）

4-2-1.导包

import rclpy
from rclpy.node import Node
#缓存所依赖的库
from tf2_ros.buffer import Buffer
#监听方所依赖的库
from tf2_ros.transform_listener import TransformListener
#定时器所依赖的库
from rclpy.time import Time

4-2-2.核心实现

# 3-1.创建一个缓存对象，融合多个坐标系相对关系为一棵坐标树；
self.buffer_ = Buffer()
# 3-2.创建一个监听器，绑定缓存对象，会将所有广播器广播的数据写入缓存；
self.tfListener_ = TransformListener(self.buffer_,self)
# 3-3.编写一个定时器，循环实现转换。
self.timer_ = self.create_wall_timer(1.0,self.on_time)
def on_time():
	# 判断是否可以实现转换
	if self.buffer.can_transform("camera","laser",Time()):
		/*
		API:lookup_transform()
		参数：
		1、希望变换到的坐标系
		2、你当前所在的坐标系，或者你想要从哪个坐标系开始变换
		3、希望获取变换关系的时间点。这通常是一个ros2::Time对象，表示一个特定的ROS 2时间戳。
		*/
		ts = self.buffer.lookup_transform("camera","laser",Time())
		 self.get_logger().info(
               "转换的结果，父坐标系:%s,子坐标系:%s,偏移量:(%.2f,%.2f,%.2f)"
               % (ts.header.frame_id,ts.child_frame_id,
               ts.transform.translation.x,
               ts.transform.translation.y,
               ts.transform.translation.z)
           )
       else:
           self.get_logger().info("转换失败......")

4-3.坐标点变换

4-3-1.头文件

//这是ROS 2的核心库。
//它包含了ROS 2节点创建、通信、回调和事件循环所需的所有基本类和函数
#include "rclcpp/rclcpp.hpp"
//tf2_ros库为ROS 2提供了坐标变换功能。
//buffer头文件包含了一个tf2缓冲区的实现，该缓冲区用于存储不同坐标系之间的变换关系。
#include "tf2_ros/buffer.h"
//transform_listener是tf2_ros库中的一个核心组件，它负责监听和缓存从其他节点发布的变换信息。
//这样，当其他部分的代码需要获取两个坐标系之间的变换时，它可以从这个缓存中查询。
#include "tf2_ros/transform_listener.h"
//这个头文件包含了与message_filters库集成的tf2消息过滤器。
//它允许你订阅一个消息流，并只在具有特定变换关系的情况下处理消息。
//这对于确保在处理消息时，相关的坐标系变换是可用的非常有用。
#include "tf2_ros/message_filter.h"
//这个头文件可能提供了一个创建ROS计时器的功能，用于定期执行某些任务。
//在tf2_ros的上下文中，这可能与定期更新或检查变换关系有关
#include "tf2_ros/create_timer_ros.h"
//PointStamped是一个包含3D点（x, y, z坐标）
//以及这个点的时间戳和参考坐标系信息的消息。
//这种消息类型常用于需要跟踪物体位置的应用。
#include "geometry_msgs/msg/point_stamped.hpp"
//这个头文件是message_filters库的一部分，它提供了一种同步多个消息流的方式。
//subscriber通常用于创建一个可以接收特定类型消息的订阅者。
#include "message_filters/subscriber.h"
//它允许你将geometry_msgs中的消息类型（如Point、Quaternion、Transform等）与tf2库中的数据类型（如tf2::Point、tf2::Quaternion、tf2::Transform等）相互转换。
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

4-3-2.核心实现

// 3-1.创建tf缓存对象指针；
std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
tf2_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
//在ROS 2中，tf2_ros::Buffer用于存储和管理不同坐标系之间的变换关系。
//这个Buffer类需要一个计时器接口来创建和管理其内部的计时器，这些计时器通常用于清理过期的tf变换。
auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
this->get_node_base_interface(),//获取ROS 2节点的基础接口，用于底层节点的通信和配置。
this->get_node_timers_interface());//获取节点的计时器接口，这个接口提供了创建、管理计时器的方法。
//将之前创建的timer_interface对象设置为tf2_buffer_的计时器接口。这样，tf2_buffer_在需要创建计时器（例如用于清理过期变换）时，就会使用这个接口来创建计时器。
tf2_buffer_->setCreateTimerInterface(timer_interface);
// 3-2.创建tf监听器；
std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(tf2_buffer_);
// 3-3.创建坐标点订阅方并订阅指定话题；
message_filters::Subscriber<geometry_msgs::msg::PointStamped> point_sub_;
point_sub_->subscribe(this,"point")
//3-4.创建消息过滤器过滤被处理的数据；
std::shared_ptr<tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped>> tf2_filter_;
/*
创建了一个MessageFilter对象，用于过滤和处理已经经过tf变换的geometry_msgs::msg::PointStamped类型的消息。
参数：
1.point_sub_:用于订阅PointStamped类型的消息
2.*tf2_buffer_:存储了不同坐标系之间的坐标关系
3.target_frame_:MessageFilter将确保所有传入的PointStamped消息都被转换到这个目标坐标系中。
4.100：是队列大小，表示可以缓存多少未处理的消息。
5.this->get_node_logging_interface()：获取当前ROS 2节点的日志接口，用于在需要时记录日志。
6.this->get_node_clock_interface()：获取当前ROS 2节点的时钟接口，用于获取时间信息
7.buffer_timeout：是超时时间，如果在这个时间内没有获取到有效的tf变换，则过滤操作会失败。
*/
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);
);
// 3-5.为消息过滤器注册转换坐标点数据的回调函数。
tf2_filter_->registerCallback(&MessageFilterPointListener::msgCallback, this);
//3-6.解析数据
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_);
	RCLCPP_INFO( this->get_logger(), "坐标点相对于base_link的坐标:(%.2f,%.2f,%.2f)", 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());
		}
}

5.坐标变换工具
tf2_ros、tf2_to

tf2_ros常用节点：
static_transform_publisher:常用于广播静态变换
tf2_monitor:常用于打印所有或特定坐标系的发布频率与网络延迟
//打印所有坐标系的发布频率和网络延迟
ros2 run tf2_ros tf2_monitor
//打印指定坐标系的发布频率与网络延迟
ros2 run tf2_ros tf2_monitor camera laser
tf2_echo:常用于特定坐标系的平移旋转关系
ros2 run tf2_ros tf2_echo world turtle1
tf2_tools常用节点：
view_frames:生成显示坐标系关系的pdf文件。
ros2 run tf2_tools view_frames