ROS TF 常用接口函数

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_ZYX
        setEuler(yaw, pitch, roll);
#else
        setRPY(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 读出转换信息，并用与坐标系转换。