ROS-TF笔记

    之前写过一篇博文<ROS-坐标转换>介绍了ROS中坐标转换的概念,并给出实例。在次基础上,本文总结了部分常用的TF的API,以供使用的时候查询。

TF 数据类型

tf::Transform
//Constructor from Quaternion (optional Vector3 )
Transform (const Quaternion &q, const Vector3 &c=Vector3(tfScalar(0), tfScalar(0), tfScalar(0)))
//Constructor from Matrix3x3 (optional Vector3)
Transform (const Matrix3x3 &b, const Vector3 &c=Vector3(tfScalar(0), tfScalar(0), tfScalar(0)))

//构造单位变换矩阵
Transform transform=tf::setIdentity ();
  • 赋值

tf::Transform transform;
transform.setOrigin( tf::Vector3(0.1, 0.0, 0.2) );//平移向量
transform.setRotation( tf::Quaternion(0,0,0,1) );//四元数
  • 获取平移和旋转

tf::Transform transform;
tf::Quaternion q=transform.getRotation();//获取四元数
tf::Vector3 t=transform.getOrigin();//获得平移向量
  • 逆变换

tf::Transform transform;
tf::Transform m=transform.inverse();//获得逆变换
  • 两个变换的乘积

mult (const Transform &t1, const Transform &t2);


tf::Vector3
tf::Vector3 v(1.0,0.0,0.0);
  • 赋值函数

tf::Vector3 v;
v.setX(1.0);
v.setY(0.0);
v.setZ(0.0);

//或通过下面的函数:
v.setValue(1.0,0.0,0.0);

v.setZero();//设置零向量
  • 获取向量的元素

tf::Vector3 v(1.0,0.0,0.0);
double x=v.getX(1.0);
double y=v.getY(0.0);
double z=v.getZ(0.0);
  • 其它函数参见文档.


tf::Quaternion
//无参数构造器
tf::Quaternion q=tf::Quaternion ();

//直接构造
Quaternion (const tfScalar &x, const tfScalar &y, const tfScalar &z, const tfScalar &w)

//通过旋转轴向量和旋转角构造
Quaternion (const Vector3 &axis, const tfScalar &angle)

//通过RPY构造
Quaternion (const tfScalar &yaw, const tfScalar &pitch, const tfScalar &roll)
  • 赋值函数

//使用Euler angles设置四元数
setEuler (const tfScalar &yaw, const tfScalar &pitch, const tfScalar &roll)

//使用Euler angles设置四元数
setEulerZYX (const tfScalar &yaw, const tfScalar &pitch, const tfScalar &roll) 

//使用旋转轴向量和旋转角度设置四元数
setRotation (const Vector3 &axis, const tfScalar &angle)

//使用RPY设置四元数
setRPY (const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)
  • 获取函数

//获取四元数表示的旋转的转角[0,2Pi]
tfScalar getAngle () const

//返回旋转轴向量
Vector3 getAxis () const
  • 其它函数

//返回一个逆旋转的四元数
Quaternion inverse ()

//四元数归一化
Quaternion 	normalized ()


tf::Matrix3x3

旋转矩阵

//无参数构造函数
Matrix3x3 ()

//通过四元数构造
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)
  • 赋值函数

//设置沿ZYX轴的旋转角度
void setEulerYPR (tfScalar eulerZ, tfScalar eulerY, tfScalar eulerX)

//设置沿ZYX的旋转角度
void setEulerZYX (const tfScalar &yaw, const tfScalar &pitch, const tfScalar &roll) 

//设置为单位矩阵
void setIdentity ()

//通过四元数设置旋转
void setRotation (const Quaternion &q)

//通过RPY旋转角设置旋转
void setRPY (tfScalar roll, tfScalar pitch, tfScalar yaw)

//直接设置旋转矩阵的值
void setValue (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)
  • 获取函数

//获取YXZ轴的旋转角
getEulerYPR (tfScalar &yaw, tfScalar &pitch, tfScalar &roll, unsigned int solution_number=1) 

//获取四元数
getRotation (Quaternion &q)

//获取RPY角度
getRPY (tfScalar &roll, tfScalar &pitch, tfScalar &yaw, unsigned int solution_number=1)

//获取逆矩阵
Matrix3x3 inverse()

//获取转置矩阵
Matrix3x3 transpose ()


tf::Stamped

这是一个模板类.


tf::StampedTransform

tf::StampedTransform 是tf::Transforms的子类,它是带时间戳的Transform.

  • 成员变量

//这个变换的子坐标
std::string child_frame_id_;
//这个变换的坐标
std::string frame_id_
//时间戳
ros::Time stamp_
  • 构造函数

StampedTransform (const tf::Transform &input, const ros::Time &timestamp, const std::string &frame_id, const std::string &child_frame_id)
  • 赋值函数

//设置Transform对象
setData (const tf::Transform &input)




TF监听器

构造函数

TransformListener(const ros::NodeHandle &nh, ros::Duration max_cache_time=ros::Duration(DEFAULT_CACHE_TIME), bool spin_thread=true)

TransformListener(ros::Duration max_cache_time=ros::Duration(DEFAULT_CACHE_TIME), bool spin_thread=true)


常用函数

canTransform()
  • 判断能否实现变换,如果出错,则返回error_msg的内容.检查在时间time,source_frame能否变换到target_frame。

bool tf::TransformListener::canTransform (const std::string &target_frame, const std::string &source_frame, const ros::Time &time, std::string *error_msg=NULL) const
  • 检查在时间source_time,source_frame能否变换到fixed_frame,那么再实现在target_time变换到target_frame。

bool tf::TransformListener::canTransform (const std::string &target_frame, const ros::Time &target_time, const std::string &source_frame, const ros::Time &source_time, const std::string &fixed_frame, std::string *error_msg=NULL) const


waitForTransform()
  • 检查在时间time, source_frame能否变换到target_frame. 它将休眠并重试每个polling_duration,直到超时的持续时间已经过去。 它不会抛出异常。 在一个错误的情况下,如果你传递一个非NULL字符串指针,它会填充字符串error_msg。

bool tf::TransformListener::waitForTransform (const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, <br>const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const
  • 测试在source_time时间,source_frame能否变换到fixed_frame,那么在target_time,变换到target_frame。

bool tf::TransformListener::waitForTransform (const std::string &target_frame, const ros::Time &target_time, const std::string &source_frame, const ros::Time &source_time, <br>const std::string &fixed_frame, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const


lookupTransform()
  • 查找两个坐标系的变换,返回的变换的方向是从target_frame到source_frame.这个方法会抛出TF异常.在时间time上使用从source_frame到target_frame的变换填充transform

void tf::TransformListener::lookupTransform (const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
  • 在source_time使用从source_frame到fixed_frame的变换填充transform,在target_time从fixed_frame到target_frame的链接变换。

void tf::TransformListener::lookupTransform (const std::string &target_frame, const ros::Time &target_time, const std::string &source_frame, const ros::Time &source_time,<br> const std::string &fixed_frame, StampedTransform &transform) const


transformDATATYPE()
  • 将数据stamped_in转换为target_frame,使用标记的数据类型中的frame_id和stamp作为源。

void tf::TransformListener::transformDATATYPE (const std::string &target_frame, const geometry_msgs::DATATYPEStamped &stamped_in, geometry_msgs::DATATYPEStamped &stamped_out) const
  • 将数据stamped_in转换为fixed_frame。 使用标记数据类型中的frame_id和stamp作为源。 然后在target_time从fixed_frame变换到target_frame。

void tf::TransformListener::transformDATATYPE (const std::string &target_frame, const ros::Time &target_time, const geometry_msgs::DATATYPEStamped &pin, <br>const std::string &fixed_frame, geometry_msgs::DATATYPEStamped &pout) const


TF广播器

构造函数

tf::TransformBroadcaster();

坐标发布

sendTransform()
void sendTransform(const StampedTransform & transform);

void sendTransform(const geometry_msgs::TransformStamped & transform);


TF异常

所有在TF里异常继承自tf::TransformException,它继承自std::runtime_error。

异常类型

  • tf::ConnectivityException
    如果由于两个坐标系ID不在同一个连接的树中而无法完成请求,则抛出。

  • tf::ExtrapolationException
    如果请求的坐标系id之间存在连接,但一个或多个变换已过期,则抛出。

  • tf::InvalidArgument
    如果参数无效则抛出。 最常见的情况是非规范化的四元数。

  • tf::LookupException
    如果引用了未发布的坐标系ID,则抛出。


TF与Eigen的转换

需要引入头文件:#include <tf_conversions/tf_eigen.h>,而且需要在CMakeLists.txt和package.xml里面加入tf_conversions的依赖,不然无法编译通过。

void tf::matrixEigenToTF (const Eigen::Matrix3d &e, tf::Matrix3x3 &t)
  • 将TF旋转矩阵转换为Eigen四元数

void tf::matrixTFToEigen (const tf::Matrix3x3 &t, Eigen::Matrix3d &e)
  • 将Eigen仿射矩阵转换为TF Pose

void tf::poseEigenToTF (const Eigen::Affine3d &e, tf::Pose &t)
  • 将Eigen变换矩阵转换为TF Transform

void tf::poseEigenToTF (const Eigen::Isometry3d &e, tf::Pose &t)
  • 将TF Pose转换为Eigen仿射矩阵

void tf::poseTFToEigen (const tf::Pose &t, Eigen::Affine3d &e)
  • 将TF Pose转换Eigen变换矩阵

void tf::poseTFToEigen (const tf::Pose &t, Eigen::Isometry3d &e)
  • 将Eigen四元数转换为TF四元数

void tf::quaternionEigenToTF (const Eigen::Quaterniond &e, tf::Quaternion &t)
  • 将TF四元数转换Eigen四元数

void tf::quaternionTFToEigen (const tf::Quaternion &t, Eigen::Quaterniond &e)
  • 将Eigen仿射矩阵转换为TF Transform

void tf::transformEigenToTF (const Eigen::Affine3d &e, tf::Transform &t)
  • 将Eigen变换矩阵转换为TF Transform

void tf::transformEigenToTF (const Eigen::Isometry3d &e, tf::Transform &t)
  • 将TF Transform转换为Eigen仿射矩阵

void tf::transformTFToEigen (const tf::Transform &t, Eigen::Affine3d &e)
  • 将TF Transform转换为Eigen变换矩阵

void tf::transformTFToEigen (const tf::Transform &t, Eigen::Isometry3d &e)
  • 将Eigen向量转换为TF向量

void tf::vectorEigenToTF (const Eigen::Vector3d &e, tf::Vector3 &t)
  • 将TF向量转换为Eigen向量

void tf::vectorTFToEigen (const tf::Vector3 &t, Eigen::Vector3d &e)


上一篇:
下一篇:

首页 所有文章 机器人 计算机视觉 自然语言处理 机器学习 编程随笔 关于