tf學習

---

學習完ROS wiki後做個簡單的筆記，連結ROS-wiki-tf-learning
以下的每點都是對應著連結中tutorial每個章節，遺忘不解時請翻看

1.tf釋出器

tf::TransformBroadcaster br;	//建立一個釋出器
tf::Transform transform;	//建立一個變換
//填入變換
tf::Quaternion q;
q.setRPY(0, 0, msg->theta);
transform.setRotation(q);
transform.setOrigin( tf::Vector3(position.x, position.y, 0.0) );
//呼叫函式釋出變換，需要的引數：變換，時間戳，參考座標系，自己的座標系
br.sendTransform(tf::StampedTransform(transform, 
				ros::Time::now(), "world", turtle_name));

2.tf監聽器

tf::TransformListener listener;
while (node.ok()){
    tf::StampedTransform transform;
    try{
    	//尋找要監聽的兩個座標系，使用transform輸出變換
      listener.lookupTransform("/turtle2", "/turtle1",  
                               ros::Time(0), transform);
    }
    catch (tf::TransformException ex){
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
    }
turtlesim::Velocity vel_msg;
    vel_msg.angular = 4.0 * atan2(transform.getOrigin().y(),
                                transform.getOrigin().x());
    vel_msg.linear = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
                                pow(transform.getOrigin().y(), 2));   

3.加入座標frame

非常像tf釋出器

  tf::TransformBroadcaster br;
  tf::Transform transform;
  ros::Rate rate(10.0);
  while (node.ok()){
    	transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
    	transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
    	//參考一下tf釋出器，以turtle1為參考系，carrot1在y軸正向2m處
    	br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));
    	rate.sleep();
  }

4.tf中的時間戳

//在這行程式碼中，ros::Time(0)代表著最新的一幀變換
listener.lookupTransform("/turtle2", "/carrot1",  
                               ros::Time(0), transform);
//如果想知道之前的變換，可以修改這個ros::Time(0)，其實一般
//來說，不是ros::Time(0)就是其他我們想要的具體時間
**********************
listener.lookupTransform("/turtle2", "/carrot1",  
                               ros::Time::now(), transform);
//這個不行，為啥呢，因為左邊監聽查詢需要幾個毫秒的時間，要求當
//前的now()，顯然不可能，會監聽失敗

  try{
    ros::Time now = ros::Time::now();
    //可以使用waitForTransform監聽到以後時間點
    listener.waitForTransform("/turtle2", "/turtle1",
                              now, ros::Duration(1.0));
    listener.lookupTransform("/turtle2", "/turtle1",
                             now, transform);

利用這個時間戳，可以進行Time travel，監聽5s前的變換

try{
    ros::Time past = ros::Time::now() - ros::Duration(5.0);
    listener.waitForTransform("/turtle2", "/turtle1",
                              past, ros::Duration(1.0));
    listener.lookupTransform("/turtle2", "/turtle1",
                             past, transform);

不僅是target frame可以使用過去的，source frame也可以

try{
    ros::Time now = ros::Time::now();
    ros::Time past = now - ros::Duration(5.0);
    listener.waitForTransform("/turtle2", now,
                              "/turtle1", past,
                              "/world", ros::Duration(1.0));
    listener.lookupTransform("/turtle2", now,
                             "/turtle1", past,
                             "/world", transform);

5.tf的除錯

可以使用tf的工具找到一些出錯的問題所在
比如想要知道某兩個變換，可以使用：
$ rosrun tf tf_echo turtle3 turtle1
如果不存在的話，會提示：

Exception thrown:Frame id /turtle3 does not exist! When trying to transform between /turtle1 and /turtle3.
The current list of frames is:
Frame /turtle1 exists with parent /world.
Frame /world exists with parent NO_PARENT.
Frame /turtle2 exists with parent /world.

也可以使用rosrun tf view_frames來生成pdf
$ rosrun tf view_frames
$ evince frames.pdf
還有一個工具：
$ rosrun tf tf_monitor turtle2 turtle1
可以看到這樣的資訊

RESULTS: for /turtle2 to /turtle1
Chain currently is: /turtle1 -> /turtle2
Net delay     avg = 0.008562: max = 0.05632

Frames:

Broadcasters:
Node: /broadcaster1 40.01641 Hz, Average Delay: 0.0001178 Max Delay: 0.000528
Node: /broadcaster2 40.01641 Hz, Average Delay: 0.0001258 Max Delay: 0.000309

6.sensor message 使用tf轉換

這一節可能用的比較多，看了好一會兒才明白這個章節是什麼意思：比如位於機器前方的雷達資料如果需要轉換到機器人base座標系下，需要怎麼做？畢竟