ROS的學習（二十一）使用時間和TF

     rosserial_arduino這個包，包括了可以在arduino上產生時間戳的一個庫，這個時間和運行roscore實例的PC或者是平板上的時間是同步的。下面使用一個例子來演示一下如何訪問時間，並且發佈一個TF變換。打開一個終端，運行arduino IDE：

arduino

     選擇File->Examples->ros_lib->TimeTF，打開的代碼如下所示：

/* 
 * rosserial Time and TF   * Publishes a transform at current time
 */

#include <ros.h>
#include <ros/time.h>
#include <tf/transform_broadcaster.h>

ros::NodeHandle  nh;

geometry_msgs::TransformStamped t;
tf::TransformBroadcaster broadcaster;

char base_link[] = "/base_link";
char odom[] = "/odom";

void setup()
{
  nh.initNode();
  broadcaster.init(nh);
}

void loop()
{  
  t.header.frame_id = odom;
  t.child_frame_id = base_link;
  t.transform.translation.x = 1.0; 
  t.transform.rotation.x = 0.0;
  t.transform.rotation.y = 0.0; 
  t.transform.rotation.z = 0.0; 
  t.transform.rotation.w = 1.0;  
  t.header.stamp = nh.now();
  broadcaster.sendTransform(t);
  nh.spinOnce();
  delay(10);
}

      下面來解釋代碼：

#include <ros.h>
#include <ros/time.h>
#include <tf/transform_broadcaster.h>

     上面的代碼是包括一些必須的頭文件，

geometry_msgs::TransformStamped t;
tf::TransformBroadcaster broadcaster;

char base_link[] = "/base_link";
char odom[] = "/odom";

     接下來實例化一個TransformStamped的消息和一個TransformBroadcaster的對象broadcaster，我們同樣需要指出所發佈變換的框架的名字。

  broadcaster.init(nh);

     在setup函數裏面，我們調用了TransformBroadcaster的初始化函數init()，且使用節點的句柄作爲一個參數。如果沒有這一步，broadcaster將不能正確的發佈消息。

  t.header.frame_id = odom;
  t.child_frame_id = base_link;
  t.transform.translation.x = 1.0; 
  t.transform.rotation.x = 0.0;
  t.transform.rotation.y = 0.0; 
  t.transform.rotation.z = 0.0; 
  t.transform.rotation.w = 1.0;

     在loop()函數中，我們填充了變換的域，各種ID都被賦予了正確的字符串類型的名字，且變換和旋轉的值被正確的設置。

 t.header.stamp = nh.now();

     調用nh.now()以ros::Time實例方式返回了當前的時間，

  broadcaster.sendTransform(t);
  nh.spinOnce();
  delay(10);
}

     最後發佈了變換，並且在再次發佈前調用了spinOnce和延遲函數。

     接下來運行程序，首先在arduino IDE中點擊upload按鈕。

     接着接着運行roscore：

roscore

     接着運行一下rosserial客戶端應用程序，它將會把arduino的消息轉發給ros的其他部分，

rosrun rosserial_python serial_node.py /dev/ttyACM0

     最後可以通過以下命令來檢查變換：

rosrun tf tf_echo odom base_link