ROS学习笔记(十四） TF介绍（二）

最新推荐文章于 2024-07-12 22:04:46 发布

bai君

最新推荐文章于 2024-07-12 22:04:46 发布

阅读量3.7k

点赞数 7

分类专栏： ROS学习笔记文章标签： c++ slam linux

本文链接：https://blog.csdn.net/weixin_44682965/article/details/107793276

版权

ROS学习笔记专栏收录该内容

20 篇文章 63 订阅

订阅专栏

TF介绍（二）

tf in c++

C++提供的TF数据类型：

名称	数据类型
向量	tf::Vector3
点	tf::Point
四元数	tf::Quaternion
3*3矩阵（旋转矩阵）	tf::Matrix3x3
位姿	tf::pose
变换	tf::Transform
带时间戳的以上类型	tf::Stamped
带时间戳的变换	tf::StampedTransform

注意tf::StampedTransform 和 geometry_msgs/TransformStamped.msg不同，tf::StampedTransform只能用在C++里，是C++的一个类，一种数据格式，不是一个消息。geometry_msgs/TransformStamped.msg是一个message，依赖于ROS，与语言无关（即是无论和种语言C++、Python、Java等，都可以发送该消息）。

数据转换：

在这里插入图片描述
TF中常见的数据转换有四元数、欧拉角、旋转矩阵之间的转换。tf中数据转化的类型都包含在#include<tf/tf.h>头文件中。
数据转换的API：
1. 定义空间点和空间向量

函数名称	函数功能
tfScalar::tfDot(const Vector3 &v1, const Vector3 &v2)	计算两个向量的点积
tfScalar length()	计算向量的模
Vector3 &normalize()	求与已知向量同方向的单位向量
tfScalar::tfAngle(const Vector3 &v1, const Vector3 &v2)	计算两个向量的夹角
tfScalar::tfDistance(const Vector3 &v1, const Vector3 &v2)	计算两个向量的距离
tfScalar::tfCross(const Vector3 &v1, const Vector3 &v2)	计算两个向量的乘积

文件位置：test/coordinate_transform.cpp

#include<ros/ros.h>
#include<tf/tf.h>
int main(int argc,char** argv)
{
	ros::init(int argc,char** argv,"coordinate_transform");
	ros::NodeHandle node;
	tf::Vector3 v1(1,1,1);
	tf::Vector3 v2(1,0,1);
	
	std::cout<<"第1部分，定义空间点和空间向量"<<std::endl;
	std::cout<<"v1 : "<<"("<<v1[0]<<","<<v1[1]<<","<<v1[2]<<"),";
	std::cout<<"v2 : "<<"("<<v2[0]<<","<<v2[1]<<","<<v2[2]<<"),";
	std::cout<<"两个向量的点积："<<tfDot(v1,v2)<<std::endl;

	std::cout<<"向量v2的模值:"<<v2.length()<<std::endl;

	tf::Vector3 v3;
	v3 = v2.normalize();
	std::cout<<"与向量v2的同方向的单位向量v3:"<<<<"("<<v3[0]<<","<<v3[1]<<","<<v3[2]<<")"<<std::endl;

	std::cout"两个向量的夹角(弧度):"<<tfAngle(v1,v2)<<std::endl;

	std::cout<<"两个向量的距离:"<<tfDistance(v1,v2)<<std::endl;
	
	tf::Vector3 v4;
	v4 = tfCross(v1,v2);
	std::cout<<"两个向量的乘积v4:"<<"("<<v4[0]<<","<<v4[1]<<","<<v4[2]<<")"<<std::endl;
	return 0;
}

2. 定义四元数

函数名称	函数功能
setRPY(const tfScalar &yaw,const tfScalar &pitch, const tfScalar &roll)	由欧拉角计算四元数
Vector3 getAxis()	由四元数得到旋转轴
setRotation(const Vector3 &axis, conat tfScalar &angle)	已知旋转轴和旋转角估计四元数

#include<ros/ros.h>
#include<tf/tf.h>

int main(int argc,char ** argv)
{
	ros::init(argc,argv,"coordinate_transformation");
	ros::NodeHandle nh;
	
	std::cout<<"第2部分，定义四元数"<<std::endl;
	tfScalar yaw,pitch,roll;
	yaw=0;
	pitch=0;
	roll=0;
	std::cout<<"欧拉角rpy("<<yaw<<","<<pitch<<","<<yaw<<")";
	tf::Quaternion q;
	q.setRPY(yaw,pitch,roll);
	std::cout<<"，转化到四元数q: "<<"("<<q[3]<<","<<q[0]<<","<<q[1]<<","<<q[2]<<")"<<std::endl;
	
	tf::Vector3 v5;
	v5 = q.getAxis();
	std::cout<<"四元数q的旋转轴v5"<<"("<<v5[0]<<","<<v5[1]<<","<<v5[2]<<")"<<std::endl;
	
	tf::Quaternion q2;
	q2 = setRotation(v5,1.570796);
	std::cout<<"旋转轴v5和旋转角度90度，转化到四元数q2:"<<"("<<q2[3]<<","<<q2[0]<<","<<q2[1]<<","<<q2[2]<<")"<<std::endl;
	return 0;
}

3. 定义旋转矩阵

函数名称	函数功能
setRotation(const Quaternion &q)	通过四元数得到旋转矩阵
getEulerYPR(tfScalar & yaw, tfScalar &pitch, tfScalar &roll)	由旋转矩阵求欧拉角

#include<ros/ros.h>
#include<tf/tf.h>

int main(int argc,char **argv)
{	
	ros::init(argc,argv,"coordinate_transform");
	ros::NodeHandle nh;
	
	std::cout<<"第3部分，定义旋转矩阵"<<std::endl;
	tf::Quaternion q(1,0,0,0);
	tf::Matrix3x3 matrix;
	tf::Vector3 v6,v7,v8;
	matrix.setRotation(q);
	v6 = matrix[0];
	v7 = matrix[1];
	v8 = matrix[2];
	std::cout<<"四元数q2对应的旋转矩阵M:"<<v6[0]<<","<<v6[1]<<","<<v6[2]<<std::endl;
	std::cout<<"                       "<<v7[0]<<","<<v7[1]<<","<<v7[2]<<std::endl;
	std::cout<<"                       "<<v8[0]<<","<<v8[1]<<","<<v8[2]<<std::endl;
	
	tfScalar m_yaw,m_pitch,m_roll;
	matrix.getEulerYPR(m_yaw,m_pitch,m_roll);
	std::cout<<"由旋转矩阵M,得到欧拉角rpy("<<m_roll<<","<<m_pitch<<","<<m_yaw<<")"<<std::endl;
	retrun 0;
}

欧拉角与四元数的互换
文件位置：test/Euler2Quaternion.cpp

#include<ros/ros.h>
#include<tf/tf.h>

int main(int argc, char** argv)
{
	ros::init(argc, argv,"Euler2Quaternion");
	ros::NodeHandle nh;
	geometry_msgs::Quaternion q;
	double roll,pitch,yaw;
	while(ros::ok())
	{
		 std::cout<<"输入的欧拉角：roll,pitch,yaw:";
		 std::cin>>roll>>pitch>>yaw;
		 q = tf::creatQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
		 std::cout<<"输出的四元数为：w = "<<q.w<<",x = "<<q.x<<",y = "<<q.y<<",z = "<<q.z<<std::endl;
		 ros::spinOnce();
	}
	return 0;
)

文件位置：test/Quaternion2Euler.cpp

#include<ros/ros.h>
#include<tf/tf.h>

int main(int argc, char** argv)
{
	ros::init(argc,argv,"Quaternion2Euler");
	ros::NodeHandle nh;	

	tf::Quaternion q;
	nav_msgs::Odometry position
	double roll,pitch,yaw;
	while(ros::ok())
	{	
		std::cout<<"输入的四元数：w,x,y,z:";
		std::cin>>position.pose.pose.orientation.w>>position.pose.pose.orientation.x>>position.pose.pose.oriention.y>>position.pose.pose.orientation.z;
		tf::quaternionMsgToTF(position.pose.pose.orientation,q);
		//法2：tf::Vector3 m_vector;
		// 	  m_vector = q.getAxis();
		tf::Matrix3x3(q).getRPY(roll,pitch,yaw);
		std::cout<<"输出的欧拉角为：roll="<<roll<<",pitch="<<pitch<<",yaw="<<yaw<<std::endl;
		
		ros::spinOnce();
	}
	return 0;
}

TF类

tf:TransformBroadcaster类

transformBroadcaster()
void sendTransform(const StampedTransform &transform)
void sendTransform(const std::vector<StampedTransform> &transform)
void sendTransform(const geometry_msgs::TransformStamped &transform)
void sendTransform(const std::vector<geometry_msgs::TransformStamped> &transform)

broadcaster相当于一个publisher。sendTransform作用就是封装publish函数。使用时，在Node中构建tf::TransformBroadcaster类，再调用sendTransform()，将transform发布到/tf的一段transform上。/tf里的transform重载了多种不同的函数类型。

文件位置：test/tf_briadcaster.cpp

#include<ros/ros.h>
#include<tf/tf.h>
#include<tf/transform_broadcaster.h>

int main(int argc,char **argv)
{
	ros::init(argc,argv,"tf_boradcaster");
	ros::NodeHandle nh;
	
	static tf::TransformBroadcaster br;
	tf::Quaternion q;
	//geometry_msgs::Quaternion qw;
	double roll=0,pitch=0,yaw=0,x=1.0,y=2.0,z=3.0;
	ros::Rate rate(1);
	while(ros::ok()):
	{
		yaw += 0.1;
		q.setRPY(roll,pitch,yaw);
		//qw.createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
		transform.setOrigin(tf::Vector3(x,y,z));
		transform.setRotation(q);
		std::cout<<"发布tf变换：sendTransform函数"<<std::endl;
		br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"base_link","link1"));
		std::cout<<"输出的四元数为：w="<<q[3]<<", x="<<q[0]<<", y="<<q[1]<<", z="<<q[2]<<std::endl;
		//std::cout<<"输出的四元数为：w="<<qw.w<<", x="<<qw.x<<", x="<<qw.y<<", y="<<qw.y<<",z="<<qw.z<<std::endl;
		rate::sleep();
		ros::spinOnce();
	}
	return 0;
}

tf::TransformListener类

void loopupTransform(const std::string &target_frame,const std::string &source_frame,const ros::Time &time,StampedTransform &transform)const
//第一个参数是目标坐标系，第二个参数为源坐标系(要得到这两个坐标系之间的转换关系)，第三个参数为查询时刻，第四个参数为存储转换关系的位置。
//第三个参数通常用ros::Time(0)，表示为最新的坐标转化关系，ros::time::now会因为延迟收发不能获取当前最新的坐标转换关系
bool canTransform()
//canTransform()是用来判断两个transform之间是否连通
bool waitForTransform()const
//waitForTransform()const是用来等待某两个transform之间的连通

文件位置：test.tf_listener.cpp

#include<ros/ros.h>
#include<tf/tf_transform.h>
#include<geometry_msgs/Twist.h>

int main(int argc, char** argv)
{
	ros::init(argc,argv,"tf_listener");
	ros::NodeHandle nh;
	
	tf::TransformLiatener listener;
	std::cout<<"1. 阻塞直到frame相通"<<std::endl;
	listener.waitTransform("base_link","link1",ros::Time(0),ros::Duration(4.0));
	ros::Rate rate(1);
	while(node.ok())
	{
		tf::StampedTransform transform;
		try
		{
			 std::cout<<"2. 监听对应的tf,返回平移和旋转"<<std::endl;
			 listener.loopupTransform("/base_link","link1",ros::Time(0),transform);
		}
		catch(tf::TransformException &ex)
		{
			ROS_ERROR("%s",ex.what());
			ros::Duration(1).sleep();
			continue;
		}
		std::cout<<"输出的位置坐标：x="<<tranform.getOrigin().x()<<",y="<<transform.getOrigin().y()<<",z="<<transform.getOrigin().z()<<std::endl;
		std::cout<<"输出的旋转四元数：w="<<transform.getRotation().getW()<<",x="<<transform.getRotation.getX()<<",y="<<transform.getRotation.getY()<<",z="<<transform.getRotation.getZ()<<std::endl;
		rate.sleep();
		rate.sleep();
	}
	return 0;
}