ROS tf2 （C++）学习笔记以及和tf对比

参考：http://wiki.ros.org/action/fullsearch/tf2/Tutorials?action=fullsearch&context=180&value=linkto%3A%22tf2%2FTutorials%22

一、如何使用tf2发布静态广播
静态广播在tf中没有被单独划分出来，可以使用下面方法发布静态tf：

  <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser4"
    args="0.0 -0.3 0.7 -1.57 0.0 0.0  /base_link /ydlidar_right_frame 40" />

同样在tf2中也存在：

 <node pkg="tf2_ros" type="static_transform_publisher" name="link1_broadcaster" 
 	args="1 0 0 0 0 0 1 link1_parent link1" />

以下展示了如何使用StaticTransformBroadcaster去发布静态tf。

1. 创建一个 learning_tf2功能包，这个功能包依赖 tf2, tf2_ros, roscpp, rospy, turtlesim

$ catkin_create_pkg learning_tf2 tf2 tf2_ros roscpp rospy turtlesim

2. 如何广播变化

static tf2_ros::StaticTransformBroadcaster static_broadcaster;
geometry_msgs::TransformStamped static_transformStamped;

static_transformStamped.header.stamp = ros::Time::now();
static_transformStamped.header.frame_id = "world";
static_transformStamped.child_frame_id = static_turtle_name;
/*
... //设置参数
*/
//发布变化
static_broadcaster.sendTransform(static_transformStamped);

3. 运行一个静态tf变化

$ rosrun learning_tf2 static_turtle_tf2_broadcaster mystaticturtle 0 0 1 0 0 0

二、如何使用tf2发布广播
在tf2中，静态广播相当于固定了旋转和平移，通过传参设定，其他流程都一样。
1.确定时间戳 2.父节点 3.孩子节点 4.平移 5.转角 6.发布

// 静态tf
 static tf2_ros::StaticTransformBroadcaster static_broadcaster;
// 动态tf
 static tf2_ros::TransformBroadcaster br;

对比tf和tf2：

void poseCallback(const turtlesim::PoseConstPtr& msg){
//创建tf2变换
    static tf2_ros::TransformBroadcaster br;
    geometry_msgs::TransformStamped transformStamped;
//创建tf变换
	static tf::TransformBroadcaster br;
	tf::Transform transform;

//tf2设定父节点孩子节点时间戳信息，tf在发布时确定	
    transformStamped.header.stamp = ros::Time::now();
    transformStamped.header.frame_id = "world";
    transformStamped.child_frame_id = turtle_name;
    
//tf2设定平移
    transformStamped.transform.translation.x = msg->x;
    transformStamped.transform.translation.y = msg->y;
    transformStamped.transform.translation.z = 0.0;
//tf设定平移
	transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );

//tf2设定转角    
    tf2::Quaternion q;
    q.setRPY(0, 0, msg->theta);
    transformStamped.transform.rotation.x = q.x();
    transformStamped.transform.rotation.y = q.y();
	transformStamped.transform.rotation.z = q.z();
	transformStamped.transform.rotation.w = q.w();
//tf设定转角
	tf::Quaternion q;
	q.setRPY(0, 0, msg->theta);
	transform.setRotation(q);
		
	br.sendTransform(transformStamped);
	br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

三、监听tf变换
1.tf2

tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer);
geometry_msgs::TransformStamped transformStamped;
transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time(0));

2.tf

tf::TransformListener listener;
tf::StampedTransform transform;
listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);

不过在tf的lookupTransform中，同样调用了tf2::ros用于得到变换

  geometry_msgs::TransformStamped output = 
    tf2_buffer_ptr_->lookupTransform(strip_leading_slash(target_frame), target_time,
                                strip_leading_slash(source_frame), source_time,
                                strip_leading_slash(fixed_frame));

四、了解tf2和时间，在使用lookupTransform函数时，在tf2树上可用
tf树随时间变化，并且tf2会为每个转换存储时间快照（默认情况下最长为10秒），到目前为止，我们一直使用lookupTransform()函数来访问该tf2树中的最新可用转换，而无需知道该转换的记录时间。那么如何在特定时间进行转换？

transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time(0));

您还可以看到我们指定的时间等于0。对于tf2，时间0表示缓冲区中的“最新可用”转换。 现在，更改此行以获取当前时间now()的转换：

transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time::now());

此时会发生错误：

[ERROR] 1253918454.307455000: Extrapolation Too Far in the future: target_time is 1253918454.307, but the closest tf2  data is at 1253918454.300 which is 0.007 seconds away.Extrapolation Too Far in the future: target_time is 1253918454.307, but the closest tf2 data is at 1253918454.301 which is 0.006 seconds away. See http://pr.willowgarage.com/pr-docs/ros-packages/tf2/html/faq.html for more info. When trying to transform between /turtle1 and /turtle2. See http://www.ros.org/wiki/tf2#Frequently_Asked_Questions
....

每个侦听器都有一个缓冲区，在其中存储来自不同tf2广播器的所有坐标转换。 当广播发出转换时，该转换进入缓冲区要花费一些时间（通常为几毫秒）。 因此，当在“现在”的时间请求帧转换时，您应该等待几毫秒，以便该信息到达。

tf2提供了一个不错的工具，可以等到转换可用为止。 通过将Duration参数添加到lookupTransform()来使用它

 transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time::now(), 
                                                ros::Duration(3.0));