tf 学习笔记，翻译

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::MessageFilter转换

这一节可能用的比较多，看了好一会儿才明白这个章节是什么意思，比如这样一个场景：机器头部前方的雷达采集到数据，需要转换到机器人base坐标系下。那么就可以使用tf::MessageFilter进行转换。

	point_sub_.subscribe(n_, "turtle_point_stamped", 10);
    tf_filter_ = new tf::MessageFilter<geometry_msgs::PointStamped>(point_sub_, tf_, target_frame_, 10);
    tf_filter_->registerCallback( boost::bind(&PoseDrawer::msgCallback, this, _1) );

订阅需要的数据，创建filter需要订阅器point_pub和tf监听器，目标坐标系。
随后进行回调函数，得到进行转换。在这个例子里面，turtle3发布的数据是以世界坐标系为参考，希望把这个数据转换到以turtle1为参考坐标系的下。

7.待续