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.待续