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 可視化坐標系