ROS tf使用

目录

• 发布tf
• 获取tf
  • lookupTransform
    • 变换关系获取
  • transformPoint
• 例程
• tf 四元数
• tf 常用类和函数
• Transform 的inv
• 参考

发布tf

1. 定义一个广播，利用它发布坐标系转换关系话题

static tf::TransformBroadcaster br；

2. 定义存Transform信息: 旋转和平移

tf::Transform transform;

2.1 设置坐标原点

transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );

setOrigin()函数的参数类型需要为tf::Vector3类型
假设是要发布一个子坐标系为”turtle1”父坐标系为“world”，那么其中(msg->x,msg->y,0.0)是指“turtle1”的坐标原点在“world”坐标系下的坐标。

2.2 设置四元数

 // 根据 欧拉角设置
tf::Quaternion q;
q.setRPY(0, 0, msg->theta);
transform.setRotation(q);
//根据四元数设置
transform.setRotation( tf::Quaternion(0, 0, 0, 1) ); 

setRPY()函数的参数为”turtle1”在“world”坐标系下的roll(绕X轴)，pitch(绕Y轴)，yaw(绕Z轴);为了确保转换正确强烈建议，在转换完后，运行下程序，打开rviz下使用确认下转换是否正确.

3. 将变换广播出去

br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));

1. transform:存储变换关系的变量；
2. ros::Time::now():广播tf使的时间戳；
3. “world”:父坐标系的名字；turtle_name:子坐标系的名字

总结一下，假设你在机器人上应用，如果你知道机器人的位置x,y,z,与三个旋转角roll,pitch,yaw就可以广播一个tf了

获取tf

通过监听tf，我们可以避免繁琐的旋转矩阵的计算，而直接获取我们需要的相关信息。
在监听中我最常用两个函数：

lookupTransform（）；
transformPoint（）；

lookupTransform

tf listener

tf::TransformListener listener; //定义监听器
tf::StampedTransform transform; //变换关系的变量

//监听两个坐标系之间的变换
 try{
    listener.lookupTransform("/turtle2", "/turtle1",
                             ros::Time(0), transform);
  }
  catch (tf::TransformException &ex) {
    ROS_ERROR("%s",ex.what());
    ros::Duration(1.0).sleep();
    continue;
  }

由于tf的会把监听的内容存放到一个缓存中，然后再读取相关的内容，而这个过程可能会有几毫秒的延迟，也就是，tf的监听器并不能监听到“现在”的变换，所以如果不使用try,catch函数会导致报错：world” passed to lookupTransform argument target_frame does not exist.

lookupTransform()函数参数说明
1./turtle1到/turtle2的转换
2.不可以把ros::Time(0)改成ros::time::now()，因为监听做不到实时，会有几毫秒的延迟。ros::Time(0)指最近时刻存储的数据，ros::time::now()则指当下，如果非要使用ros::time::now,则需要结合waitForTransform()使用，具体见:Wait for transforms
3.最后转换关系存入transform中

变换关系获取

//turtle1坐标系的原点，在turtle2坐标系下的位置
transform.getOrigin().x()
transform.getOrigin().y()

transform.getRotation().getW();
transform.getRotation().getX();
transform.getRotation().getY();
transform.getRotation().getZ();

transformPoint

在实际应用中我们肯定会需要把在一个坐标系下的点转换到另一个坐标系下，这就需要transformPoint()函数。

 listener_.transformPoint("PTAM_world",m_normal_pose,pose_PTAM_world)；

1. 其中m_normal_pose数据类型为 geometry_msgs::PointStamped，其中需要定义m_normal_pose.header.frame_id即该点所属的坐标系
2. 而PTAM_world则指，我要将m_normal_pose转换到PTAM_world坐标系下。
   pose_PTAM_world是转换的结果，数据类型同样为geometry_msgs::PointStamped。

例程

geometry_msgs::PointStamped turtle1;
turtle1.header.stamp=ros::Time();
turtle1.header.frame_id="turtle1";
turtle1.point.x=1;
turtle1.point.y=2;
turtle1.point.z=3;
geometry_msgs::PointStamped turtle1_world;
try{
  listener_.transformPoint("PTAM_world",turtle1,turtle1_world);
}
catch (tf::TransformException &ex) {
  ROS_ERROR("%s",ex.what());
  ros::Duration(1.0).sleep();
}

同样因为延时，turtle1.header.stamp不能为ros::Time::now();否则会出类似错误

[ERROR] [1456669076.279804500]: Lookup would require extrapolation into the future. Requested time 1456669076.279616253 but the latest data is at time 1456669076.159341977, when looking up transform from frame …

tf 四元数

 tf::Quaternion quat;
 tf::quaternionMsgToTF(amcl_pose.pose.pose.orientation,quat);
 double roll,pitch,yaw;
 tf::Matrix3x3(quat).getRPY(roll,pitch,yaw);
//获取偏航角
tf::getYaw(msg->pose.pose.orientation);

tf 常用类和函数

geometry_msgs::PoseStamped
消息简介：内容包括序列号、时间戳、frameID、位姿（点表示位置和四元数表示姿态）

tf::Transform和tf::pose完全一样，就是一个typedef
类简介：储存了3×3旋转矩阵和3×1平移矢量。

tf::StampedTransform
类简介：保存stamped transform类型的数据。带有ROS时间戳、子坐标系、父坐标系、旋转矩阵（3×3）和平移矢量（3×1）。

tf::TransformListener::waitForTransform() tf::TransformListener::lookupTransform()
简介：这两个函数通常一起使用，因为lookup这个函数读变换矩阵时需指定ros::time::now();但通常转换需要时间，wait函数可以等待转换好后，然后使用lookup。

tf::TransformListener::transformPose（）
函数简介：转换位姿，将某位姿转换到指定坐标系下的位姿表示。

tf::Stamped 模板
tf::Stamped 对数据类型做模板化（除了tf::Transform），并附带元素frame_id_和stamp_ 。

tf::poseStampedMsgToTF
函数简介：将PoseStamped msg 转换到 Stamped

Transform 的inv

Transform inverse() const
{ 
  Matrix3x3 inv = m_basis.transpose();
  return Transform(inv, inv * -m_origin);
}

//calculate the transformation between both poses (difference between both)
tf::Pose diff = first.inverse() * second;
double delta_x = diff.getOrigin().getX();
double delta_y = diff.getOrigin().getY();
double delta_phi = tf::getYaw(diff.getRotation());

first 和 second在相同世界坐标系world坐标系内，计算first.inv()，计算出来world到first坐标系的转换$W^w_{f}$，之后既可以计算出来second在first坐标系下的相对位置关系

参考

tf 使用
wiki tf toturials
tf中的常用类和函数
Rviz 可视化坐标系