ROS坐标转换工具

Shilong Wang

已于 2022-11-29 12:53:36 修改

阅读量3.1k

点赞数 8

分类专栏： ROS 文章标签：机器人 c++

于 2022-09-07 15:09:01 首次发布

本文链接：https://blog.csdn.net/wsl_longwudi/article/details/126746367

版权

ROS 专栏收录该内容

17 篇文章

订阅专栏

tf2

API手册

tf2定义的数据

头文件：#include<tf2/LinearMath/...>

与geometry_msgs的消息类型有很好的对应关系，并可以通过tf2_geometry_msgs包进行转化。

tf2::Quaternion 对应 geometry_msgs::Quaternion

tf2::Vector3 对应 geometry_msgs::Vector3 geometry_msgs::Point

tf2::Transform 对应 geometry_msgs::Pose geometry_msgs::Transform

tf2::Stamped 模板类

给数据类型带上header，将tf2的数据类型与geometry_msgs的Stamped对应。

公共属性

std::string frame_id_
The frame_id associated this data.
ros::Time stamp_
The time stamp associated with this data.

公共成员函数

Stamped<T> & operator= (const Stamped<T> &rhs)
void setData (const T &input){*static_cast<T*>(this) = input;};
Stamped()
Stamped (const Stamped<T> &s)
Stamped (const T &input, const ros::Time &timestamp, const std::string &frame_id)

tf2::Stamped<tf2::Quaternion> 对应 geometry_msgs::QuaternionStamped

tf2::Stamped<tf2::Vector3> 对应 geometry_msgs::Vector3Stamped geometry_msgs::PointStamped

tf2::Stamped<tf2::Transform> 对应 geometry_msgs::PoseStamped geometry_msgs::TransformStamped

对应的数据类型均可以通过tf2_geometry_msgs包的tf2::fromMsg和tf2::toMsg函数转化。

跳转链接：tf2_geometry_msgs

Quaternion

头文件

#include "tf2/LinearMath/Quaternion.h"

成员函数

tf2Scalar tf2::Quaternion::getX();//获取x分量
tf2Scalar tf2::Quaternion::getY();//获取y分量
tf2Scalar tf2::Quaternion::getZ();//获取z分量
tf2Scalar tf2::Quaternion::getW();//获取w分量
Quaternion tf2::Quaternion::inverse();//求逆
tf2Scalar tf2::Quaternion::angle(const Quaternion& q)；//返回两个四元数表示的转角的中间角

对于四元数，有两种数据类型：msg 和 tf

geometry_msgs::Quaternion quat_msg //这种声明的就是 msg 类型
tf tf2::Quaternion myQuaternion //这种声明的就是 tf 类型

在tf2_geometry_msgs.h ⽂件中提供了数据类型转换的函数

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
tf2::Quaternion quat_tf;
geometry_msgs::Quaternion quat_msg;
tf2::convert(quat_msg,quat_tf);//将msg类型转换为tf类型
tf2::fromMsg(quat_msg,quat_tf);//将msg类型转换为tf类型
quat_msg=tf2::toMsg(quat_tf);//将tf类型转换为msg类型

四元数与欧拉角的转换

四元数转欧拉角

void tf2::Quaternion::setRPY(const tf2Scalar &roll,
                             const tf2Scalar &pitch,
                             const tf2Scalar &yaw );
void tf2::Quaternion::setEulerZYX(const tf2Scalar &yaw,
                                  const tf2Scalar &pitch,
                                  const tf2Scalar &roll){
    setRPY(roll,pitch,yaw);
}

设置表示以下旋转的四元数

先绕物体的Z轴旋转偏航角yaw
绕旋转之后的Y轴旋转俯仰角pitch
绕旋转之后的X轴旋转滚转角roll// Transform from global_frame_ to map_frame_

Parameters
- roll : Angle around X
- pitch : Angle around Y
- yaw : Angle around Z

欧拉角转四元数

API

tf2::Quaternion quat;
double roll,pitch,yaw;//定义存储rpy的容器
//法一
#include <tf2/impl/utils.h>
void tf2::impl::getEulerYPR(const tf2::Quaternion &q,double &yaw,double &pitch,double &roll);
double tf2::impl::getYaw(const tf2::Quaternion &q);//getEulerYPR的简化版
//法二
tf2:Matrix3x3 m(quat);
m.getRPY(roll,pitch,yaw);//进行转换

坐标转换

API

tf会对非记录点的中间时刻进行插值计算。

最重要的三个类：坐标转换发布者tf2_ros::TransformBroadcaster，坐标转换监听者tf2_ros::TransformListener，坐标转换监听者缓存区tf2_ros::Buffer。tf2_ros::TransformListener和tf2_ros::Buffer配合使用实现坐标监听和坐标转换，tf2_ros::TransformListener只有一个构造函数，负责ROS中订阅坐标树话题，tf2_ros::Buffer用来缓存从坐标树话题接收的坐标转换信息，其提供了所有的坐标监听和坐标转换的API。

另外还有专门针对静态坐标转换优化的tf2_ros::StaticTransformBroadcaster，不再保留时间历史，节省存储和查找时间。

tf2_ros::TransformBroadcaster

#include <tf2_ros/transform_broadcaster.h>
void TransformBroadcaster::sendTransform(const geometry_msgs::TransformStamped & msgtf)
void TransformBroadcaster::sendTransform(const std::vector<geometry_msgs::TransformStamped> & msgtf)

tf2_ros::TransformListener

此类提供了一种请求和接收坐标系变换信息的简单方法

#include <tf2_ros/transform_listener.h>
TransformListener(tf2::BufferCore &buffer, bool spin_thread=true, ros::TransportHints transport_hints=ros::TransportHints())
TransformListener(tf2::BufferCore &buffer, const ros::NodeHandle &nh, bool spin_thread=true, ros::TransportHints transport_hints=ros::TransportHints())

spin_thread：是否单开回调线程

tf2_ros::Buffer

Buffer (ros::Duration cache_time=ros::Duration(BufferCore::DEFAULT_CACHE_TIME), bool debug=false);
//cache_time:保存多长时间的历史坐标变换

检测坐标转换是否可以提供

//普通API，查询目标时刻的两个坐标系的坐标转换
bool tf2_ros::Buffer::canTransform (const std::string &target_frame,
                                    const std::string &source_frame,
                                    const ros::Time &target_time,
                                    const ros::Duration timeout,
                                    std::string *errstr = NULL )	
//高级API，查询目标时刻的目标坐标系到源时刻的源坐标系的坐标转换
bool tf2_ros::Buffer::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,
                                    const ros::Duration timeout,
                                    std::string *errstr = NULL )

Parameters

target_frame

目标坐标系
target_time

需要转换的目标坐标系的时刻
source_frame

源坐标系
source_time

需要转换的源坐标系的时刻
fixed_frame

不随时间变化的固定坐标系，比如“world”，“map”
timeout

How long to block before failing
errstr

A pointer to a string which will be filled with why the transform failed, if not NULL

查询坐标变换并缓存到缓存区

//普通API，查询目标时刻的两个坐标系的坐标转换
geometry_msgs::TransformStamped tf2_ros::Buffer::lookupTransform(const std::string &target_frame,
                                                                 const std::string &source_frame,
                                                                 const ros::Time &time,
                                                                 const ros::Duration timeout )
//高级API，查询源时刻的源坐标系到目标时刻的目标坐标系的坐标转换(源坐标系相对目标坐标系的偏移)
geometry_msgs::TransformStamped tf2_ros::Buffer::lookupTransform(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,
                                                                 const ros::Duration timeout )

参数
- time
  
  需要坐标转换的时间点(0代表最新的)
- target_frame
  
  目标坐标系
- target_time
  
  需要转换的目标坐标系的时刻(0代表最新的)
- source_frame
  
  源坐标系
- source_time
  
  需要转换的源坐标系的时刻(0代表最新的)
- fixed_frame
  
  不随时间变化的固定坐标系，比如“world”，“map”
- timeout
  
  How long to block before failing
返回值：代表坐标转换的TransformStamped消息

通常用法

tf2_ros::Buffer buffer(ros::Duration(10));
tf2_ros::TransformListener tfListener(buffer);
try
{
    transform = buffer->lookupTransform(map_frame_, global_frame_, ros::Time(0));
}
catch (tf2::TransformException ex)
{
    ROS_ERROR("%s", ex.what());
    return;
}

执行坐标变换

注意：Vector3和Point的坐标变换是不同的，Vector3是矢量，坐标变换只改变方向，而Point是坐标点，既旋转又平移变换。

坐标转换乘法

geometry_msgs::TransformStamped transform;
transform = buffer->lookupTransform(map_frame, global_frame, ros::Time(0));
tf2::Transform tf2_transform;
tf2::convert(transform.transform, tf2_transform);
tf2::Vector3 p(x,y,z);
p = tf2_transform * p;// Transform from global_frame to map_frame

tf2::doTransform API

template<class T >
void tf2::doTransform( const T &data_in, T &data_out,const geometry_msgs::TransformStamped &transform )

Buffer成员函数transformAPI

内部也是调用tf2::doTransformAPI实现

//返回值作为输出
template <class T>
     T& transform(const T& in, T& out, 
                  const std::string& target_frame, ros::Duration timeout=ros::Duration(0.0)) const

template<class T >//简单API，同一时间点的转换
T tf2_ros::BufferInterface::transform(const T &in,
                                      const std::string &target_frame,
                                      ros::Duration timeout = ros::Duration(0.0) )	
template<class T >//高级API，允许在不同时间点之间进行转换，并指定不随时间变化的固定帧
T tf2_ros::BufferInterface::transform(const T &in,
                                      const std::string &target_frame,
                                      const ros::Time &target_time,
                                      const std::string &fixed_frame,
                                      ros::Duration timeout = ros::Duration(0.0) )
//参数引用作为输出
template<class T >//简单API，同一时间点的转换
T& tf2_ros::BufferInterface::transform(const T &in, T &out, 
                                       const std::string &target_frame,
                                       ros::Duration timeout = ros::Duration(0.0) )
{
    // do the transform
    tf2::doTransform(in, out, lookupTransform(target_frame, tf2::getFrameId(in), tf2::getTimestamp(in), timeout));
    return out;
}
template<class T >//高级API，允许在不同时间点之间进行转换，并指定不随时间变化的固定帧
T& tf2_ros::BufferInterface::transform(const T &in, T &out, 
                                       const std::string &target_frame,
                                       const ros::Time &target_time,
                                       const std::string &fixed_frame,
                                       ros::Duration timeout = ros::Duration(0.0) )	
{
    // do the transform
    tf2::doTransform(in, out, lookupTransform(target_frame, target_time, 
                                              tf2::getFrameId(in), tf2::getTimestamp(in), 
                                              fixed_frame, timeout));
    return out;
}
//将结果转换为指定的输出类型，输入类型A，输出类型B
template<class A , class B >//简单API，同一时间点的转换
B& tf2_ros::BufferInterface::transform(const A &in, B &out,
                                       const std::string &target_frame,
                                       ros::Duration timeout = ros::Duration(0.0) )
template<class A , class B >//高级API，允许在不同时间点之间进行转换，并指定不随时间变化的固定帧
B& tf2_ros::BufferInterface::transform(const A &in, B &out,
                                       const std::string &target_frame,
                                       const ros::Time &target_time, 
                                       const std::string &fixed_frame,
                                       ros::Duration timeout = ros::Duration(0.0) )

模板类型可以是Transform、Pose、Vector、Quaternion(在geometry_msgs中定义的)

tf2_geometry_msgs

API手册

定义函数

坐标转换函数：tf2::doTransform

格式转换函数：tf2::convert

template<class A , class B >
void tf2::convert( const A &a,B &b )//从A转换为B，底层依靠fromMsg和toMsg实现，需有对应类型的fromMsg或toMsg定义

geometry_msgs转化为tf2数据类型：tf2::fromMsg(geometry_msgs::A,tf2::B)
tf数据类型转化为geometry_msgs：geometry_msgs::B = tf2::toMsg(tf2::A)
从geometry_msgs消息的header中提取frame id：tf2::getFrameId
从geometry_msgs消息的header中提取stamp时间：tf2::getTimestamped

tf2_eigen

头文件#include<tf2_eigen/tf2_eigen.h>
包含eigen和geometry_msgs数据类型的相互转换

tf2命名空间

API手册

geometry_msgs::TransformStamped eigenToTransform (const Eigen::Isometry3d &T)
geometry_msgs::TransformStamped eigenToTransform (const Eigen::Affine3d &T)
Eigen::Isometry3d transformToEigen (const geometry_msgs::Transform &t)
Eigen::Isometry3d transformToEigen (const geometry_msgs::TransformStamped &t)

注意：调用上述方法需要加tf2::

Eigen命名空间

void fromMsg (const geometry_msgs::Point &msg, Eigen::Vector3d &out)
void fromMsg (const geometry_msgs::Pose &msg, Eigen::Affine3d &out)
void fromMsg (const geometry_msgs::Pose &msg, Eigen::Isometry3d &out)
void fromMsg (const geometry_msgs::Quaternion &msg, Eigen::Quaterniond &out)
void fromMsg (const geometry_msgs::Twist &msg, Eigen::Matrix< double, 6, 1 > &out)
geometry_msgs::Pose toMsg (const Eigen::Affine3d &in)
geometry_msgs::Pose toMsg (const Eigen::Isometry3d &in)
geometry_msgs::Twist toMsg (const Eigen::Matrix< double, 6, 1 > &in)
geometry_msgs::Quaternion toMsg (const Eigen::Quaterniond &in)
geometry_msgs::Point toMsg (const Eigen::Vector3d &in)

注意：调用上述方法需要加Eigen::

发布静态坐标变换

<launch>
   <node pkg="tf2_ros" type="static_transform_publisher" args="x y z yaw pitch roll frame_id child_frame_id period_in_ms" />
   <node pkg="tf2_ros" type="static_transform_publisher" args="x y z qx qy qz qw frame_id child_frame_id period_in_ms" />
</launch>