tf常见函数接口用法

1.广播一个tf消息，transform旋转的组成，由欧拉角转到四元数，平移向量等。

tf::TransformBroadcaster baselink_to_laserlink_broadcaster;geometry_msgs::TransformStamped bl_trans;   //这个格式又是什么组成的
bl_trans.header.stamp = ros::Time::now();
bl_trans.header.frame_id = "base_link";
bl_trans.child_frame_id = "laser_link";bl_trans.transform.translation.x = 1;
bl_trans.transform.translation.y = 1;
bl_trans.transform.translation.z = 0.0;
bl_trans.transform.rotation = tf::createQuaternionMsgFromYaw(M_PI/4);baselink_to_laserlink_broadcaster.sendTransform(bl_trans);

2. 监听两坐标系是否能转换，将一个向量从原坐标系转换到目标坐标系下

tf::TransformListener* listener=NULL;
listener=new (tf::TransformListener);
listener->waitForTransform("/base_link", "laser_link", ros::Time(0), ros::Duration(10.0));geometry_msgs::PointStamped P_l;
P_l.header.frame_id = "laser_link";
P_l.point.x=sqrt(2.0)/2.0;
P_l.point.y=sqrt(2.0)/2.0;
P_l.point.z=0;point_pub_l.publish(P_l);geometry_msgs::PointStamped P_b;
listener->transformPoint("base_link",P_l,P_b);

tf 常用的ROS 指令

1.查看坐标转换关系

rosrun tf tf_echo /map /odom
At time 1263248513.809
- Translation: [2.398, 6.783, 0.000]
- Rotation: in Quaternion [0.000, 0.000, -0.707, 0.707]
in RPY [0.000, -0.000, -1.570]

tf::Vector3 类的使用,默认它是一个列向量

定义和类赋值：

tf::Vector3 point_in_base2(1, 2, 0);
//重新赋值
point_in_base2.setX(2);
point_in_base2.sety(3);
point_in_base2.setX(4);//获取x,y,z 值
point_in_base2.getX();
point_in_base2.getY();
point_in_base2.getZ();
或者
point_in_base2.x();
point_in_base2.y();
point_in_base2.z();

之后还会有如下操作函数
向量的点乘
向量的叉乘cross
向量+，-，*,除运算符重载
向量的求模(长度)，归一化normalize(向量除以自己的长度，得到相同方向，长度为1的向量)
向量求夹角

tf::Matrix3x3 类

它是一个3x3的矩阵，有3个Vector3 m_el[3]，(此时使用的是行向量)

定义及赋值
//用四元数构造3x3旋转矩阵
Matrix3x3(const Quaternion& q)
Matrix3x3(const tfScalar& xx, const tfScalar& xy, const tfScalar& xz,const tfScalar& yx, const tfScalar& yy, const tfScalar& yz,const tfScalar& zx, const tfScalar& zy, const tfScalar& zz)
//赋值构造函数
Matrix3x3 (const Matrix3x3& other)常用的函数
//四元数转旋转矩阵
void setRotation(const Quaternion& q)
//欧拉角 转旋转矩阵
setRPY(tfScalar roll, tfScalar pitch,tfScalar yaw)
//旋转矩阵转四元数
getRotation(Quaternion& q)
// 旋转矩阵转欧拉角
getRPY(tfScalar& roll, tfScalar& pitch, tfScalar& yaw, unsigned int solution_number = 1)
//矩阵转置
Matrix3x3 transpose() const;
//矩阵求逆
Matrix3x3 inverse() const;

还有如下函数操作
运算符重载=，*=

tf::Quaternion 四元数类

tf::Quaternion q;
q.setRPY(0,0,3.14159);定义及赋值
Quaternion(const tfScalar& x, const tfScalar& y, const tfScalar& z, const tfScalar& w)
//通过欧拉角设置四元数，这里要分情况，
Quaternion(const tfScalar& yaw, const tfScalar& pitch, const tfScalar& roll){
#ifndef TF_EULER_DEFAULT_ZYXsetEuler(yaw, pitch, roll);
#elsesetRPY(roll, pitch, yaw);  //一般使用这个
#endif}
//通过绕axis轴旋转angle
Quaternion(const Vector3& axis, const tfScalar& angle)常用函数setRPY(const tfScalar& roll, const tfScalar& pitch, const tfScalar& yaw)
getX(),getY(),getZ()
x(),y(),z(),w()

tf::Transform 类

它是一个含有
Matrix3x3 m_basis; //3x3旋转矩阵
Vector3 m_origin; //平移向量
的类

常见用法：
//定义一个坐标旋转和平移的Transform
tf::Transform laserlink_in_baselink;
laserlink_in_baselink.setOrigin(tf::Vector3(1,1,0));
tf::Quaternion q;
q.setRPY(0,0,M_PI/4);
laserlink_in_baselink.setRotation(q);//Transform*Vector3,表明把一个点转换到另一个坐标系
tf::Vector3 point_in_laser(sqrt(2)/2, sqrt(2)/2, 0);
tf::Vector3 point_in_base=laserlink_in_baselink * point_in_laser;定义及赋值
Transform(const Quaternion& q,const Vector3& c = Vector3(tfScalar(0), tfScalar(0), tfScalar(0)))Transform(const Matrix3x3& b,const Vector3& c = Vector3(tfScalar(0), tfScalar(0), tfScalar(0)))Transform (const Transform& other)
Transform& operator=(const Transform& other)常见函数
setOrigin(const Vector3& origin)
getOrigin()setRotation(const Quaternion& q)
getRotation()inverse() //这个求逆和3x3矩阵有点不同
Transform inverse() const
{Matrix3x3 inv = m_basis.transpose();return Transform(inv, inv * -m_origin);
}//This = Transform1 * Transform2，这里是最容易乱的，
mult(const Transform& t1, const Transform& t2)
{m_basis = t1.m_basis * t2.m_basis;m_origin = t1(t2.m_origin); //Transform * Vector3
}
operator()(const Vector3& x) const
{return Vector3(m_basis[0].dot(x) + m_origin.x(),m_basis[1].dot(x) + m_origin.y(),m_basis[2].dot(x) + m_origin.z());
}

tf 类的常见用法

bool setTransform(const StampedTransform& transform, const std::string & authority = "default_authority");void lookupTransform(const std::string& target_frame, const std::string& source_frame,const ros::Time& time, StampedTransform& transform) const;bool waitForTransform(const std::string& target_frame, const std::string& source_frame,const ros::Time& time, const ros::Duration& timeout, const ros::Duration& polling_sleep_duration = ros::Duration(0.01),std::string* error_msg = NULL) const;void transformVector(const std::string& target_frame, const Stamped<tf::Vector3>& stamped_in, Stamped<tf::Vector3>& stamped_out) const;void transformPoint(const std::string& target_frame, const Stamped<tf::Point>& stamped_in, Stamped<tf::Point>& stamped_out) const;void transformPose(const std::string& target_frame, const Stamped<tf::Pose>& stamped_in, Stamped<tf::Pose>& stamped_out) const;//获取tf 前缀，这怎么用？
std::string getTFPrefix() const { return tf_prefix_;};tf::TransformBroadcaster 广播tf转换关系
//仅参数不一样，为什么又要在geometry_msgs里面起一个TransformStamped ?? ,有什么区别吗？
TransformBroadcaster::sendTransform(const geometry_msgs::TransformStamped & msgtf)
TransformBroadcaster::sendTransform(const StampedTransform & transform)常见用法:
tf::TransformBroadcaster baselink_to_laserlink_broadcaster;geometry_msgs::TransformStamped bl_trans;   //这个格式又是什么组成的
bl_trans.header.stamp = ros::Time::now();
bl_trans.header.frame_id = "base_link";
bl_trans.child_frame_id = "laser_link";bl_trans.transform.translation.x = 1;
bl_trans.transform.translation.y = 1;
bl_trans.transform.translation.z = 0.0;
bl_trans.transform.rotation = tf::createQuaternionMsgFromYaw(M_PI/4);baselink_to_laserlink_broadcaster.sendTransform(bl_trans);
//这函数最终是跳到tf2_ros 包中的TransformBroadcaster
//tf2_ros::TransformBroadcaster tf2_broadcaster_;
//tf2_broadcaster_.sendTransform(msgtf);

tf::TransformListener 类的使用
常见用法:

tf::TransformListener* listener=NULL;
listener=new (tf::TransformListener);
listener->waitForTransform("/base_link", "laser_link", ros::Time(0), ros::Duration(10.0));
/geometry_msgs::PointStamped P_l;P_l.header.frame_id = "laser_link";P_l.point.x=sqrt(2.0)/2.0;P_l.point.y=sqrt(2.0)/2.0;P_l.point.z=0;point_pub_l.publish(P_l);geometry_msgs::PointStamped P_b;//P_b.header.frame_id = "base_link";listener->transformPoint("base_link",P_l,P_b);//获取节点空间 "~" ??
inline std::string getPrefixParam(ros::NodeHandle & nh) {std::string param;if (!nh.searchParam("tf_prefix", param))return "";std::string return_val;nh.getParam(param, return_val);return return_val;
}// 用tf_prefix去掉frame_id 的前缀,如"/map"去掉"/"std::string resolve(const std::string& frame_name){ros::NodeHandle n("~");std::string prefix = tf::getPrefixParam(n);return tf::resolve(prefix, frame_name);};常见函数transformVector(const std::string& target_frame, const geometry_msgs::Vector3Stamped& stamped_in, geometry_msgs::Vector3Stamped& stamped_out)transformPoint(const std::string& target_frame, const geometry_msgs::PointStamped& stamped_in, geometry_msgs::PointStamped& stamped_out)transformPose(const std::string& target_frame, const geometry_msgs::PoseStamped& stamped_in, geometry_msgs::PoseStamped& stamped_out)transformPointCloud(const std::string& target_frame, const sensor_msgs::PointCloud& pcin, sensor_msgs::PointCloud& pcout)//它也是用tf2_ros::TransformListener tf2_listener_; 最终跳到tf2_ros 下去实现这些包transform_datatypes.h 中主要是一些数据类型转换，将tf ，transform 与 geometry_msgs::transform 转换等
tf::StampedTransform 是带有时间戳，frame_id_ 和 child_frame_id_ 的Transform//通过四元数 获取欧拉角
static inline double getYaw(const Quaternion& bt_q){tfScalar useless_pitch, useless_roll, yaw;tf::Matrix3x3(bt_q).getRPY( useless_roll, useless_pitch,yaw);return yaw;
}//将正常的四元数 转换到 geometry_msgs::Quaternion，
quaternionTFToMsg(const Quaternion& bt, geometry_msgs::Quaternion& msg)
getYaw(const geometry_msgs::Quaternion& msg_q)//他们区别不大，做了一个长度限制tf::Quaternion createQuaternionFromRPY(double roll,double pitch,double yaw)Quaternion createQuaternionFromYaw(double yaw)
geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw)quaternionStampedMsgToTF(const geometry_msgs::QuaternionStamped & msg, Stamped<Quaternion>& bt)
quaternionStampedTFToMsg(const Stamped<Quaternion>& bt, geometry_msgs::QuaternionStamped & msg)transformMsgToTF(const geometry_msgs::Transform& msg, Transform& bt)
{bt = Transform(Quaternion(msg.rotation.x, msg.rotation.y, msg.rotation.z, msg.rotation.w), Vector3(msg.translation.x, msg.translation.y, msg.translation.z));};transformTFToMsg(const Transform& bt, geometry_msgs::Transform& msg)
{vector3TFToMsg(bt.getOrigin(), msg.translation);  quaternionTFToMsg(bt.getRotation(), msg.rotation);};transformStampedMsgToTF(const geometry_msgs::TransformStamped & msg, StampedTransform& bt)
transformStampedTFToMsg(const StampedTransform& bt, geometry_msgs::TransformStamped & msg)

为什么要这样转换数据
因为ros发布的tf消息都是geometry_msgs::TransformStamped 类型的，所以一般会把tf， transform 转成geometry_msgs 下的消息，在转发出去

如何从tf 读出转换信息，并用与坐标系转换。

ROS TF 常用接口函数相关推荐

【嵌入式】Libmodbus源码分析(二)-常用接口函数分析
00. 目录文章目录 00. 目录 01. modbus常用接口函数概述 02. modbus辅助接口函数 03. modbus功能接口函数 04. modbus数据处理 05. 附录 01. mo ...
【LeetCode 总结】Leetcode 题型分类总结、索引与常用接口函数
文章目录零. Java 常用接口函数一. 动态规划二. 链表三. 哈希表四. 滑动窗口五. 字符串六. DFS.BFS 七. 二分法八. 二叉树九. 偏数学.过目不忘 and 原地算 ...
加密机PKCS#11国际加密标准解读-概念和常用接口函数（1）
PKCS#11是使用非常普遍的密码设备接口,在实际应用中,国密的密码设备应用接口规范GMT0018与之作用相同,在技术体系架构中处于类似的位置. 在密码产品的开发中,按照PKCS#11或者GMT001 ...
版本字符串比较工具接口常用接口函数
版本升级比较常用的接口,字符串解析,不是很难,但没必须重复造轮子,保存一份网上搜到的实现: /*** 比较版本号的大小,前者大则返回一个正数,后者大返回一个负数,相等则返回0** @param ver ...
QList模板类常用接口函数
插入操作:insert() 函数原型:void QList::insert(int i, const T &value) 在索引后插入值 i:索引 value:插入值 Example: QLi ...
roscpp中常用的函数、类以及命名空间
roscpp中常用的函数.类以及命名空间前言前言经过了一段时间的ros与gazebo机器人编队仿真的学习,对ros的结构有了一定了解,接下来将对松灵机器人的底盘进行实车开发,主要开发语言使用C+ ...
记录之 tensorflow中几个常用的函数：tf.unstack，tf.concat() 和 tf.stack() 等
1.tf.to_int32():tf.to_float()等函数,主要是强制类型转换函数: 2.tf.shape(tensor):获取tensor的尺寸 3.tf.round(a):四舍五入函数,张 ...
python常用导入函数及其他操作备忘录
python常用导入函数及其他操作备忘录 python常用导入函数解压缩zip并读取csv文件查看缺失值权重系数取绝对值后排序(查看特征权重重要度) python常用导入函数 from IPyt ...
windows系统中创建线程常用哪几个函数，有什么区别？
windows系统中创建线程常用哪几个函数? 在windows系统中创建线程的函数有: CreadThread,AfxBeginThread,_beginthread,_beginthreadex. ...

ROS TF 常用接口函数