ROS tf
http://wiki.ros.org/navigation/Tutorials/RobotSetup/TF
1. 以两个参考系为例,定义和存储参考系之间的关系。定义两个节点分别对应于base_link和base_laser,首先要确定哪个节点作为母节点,那个作为子节点。子节点通过tf即可自动转化为母节点坐标系下的坐标
2. 代码实现
1. 广播两个参考系之间的tf转换关系
2.订阅tf,然后从tf树遍历到两参考系变换公式,进行计算变换
A.广播
#include<tf/transform_broadcaster.h>
(1) 生成一个tf::TransformBroadcaster 对象(broadcaster)
(2) 发布转换关系:
broadcaster.sendTransform(
tf::StampedTransform(
tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
ros::Time::now(),"base_link", "base_laser"));
**这边也可以直接使用tf::Transform 类,他包含了Quaternion和Vector3
tf::Transform transform;
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
// transform.setOrigin( tf::Vector3(0.1,0.0, 0.)
tf::Quaternion q;
q.setRPY(0, 0, msg->theta);// q.setRPY(0,0, 0)
transform.setRotation(q);
//http://docs.ros.org/jade/api/tf/html/c++/classtf_1_1Transform.html
(3) 关键在于StampedTransform(),包含五个参数,首先是两个参考系之间的旋转变换,通过Quaternion四元数来存储旋转变换的参数,因为我们用到的两个参考系没有发生旋转变换,所以倾斜角、滚动角、偏航角都是0。第二个参数是坐标的位移变换,我们用到的两个参考系在X轴和Z轴发生了位置,根据位移值填入到btVector3 向量中。第三个参数是时间戳。第四个参数是母节点存储的参考系,即base_link,最后一个参数是子节点存储的参考系,即base_laser。
B. 订阅并完成转化
#include<tf/transform_listener.h>
(1) 生成一个tf::TransformListener对象,该对象会自动订阅ROS中的tf消息,并且管理所有的变换关系数据。
ros::Timer timer = n.createTimer(ros::Duration(1.0),boost::bind(&transformPoint, boost::ref(listener)));
(2) 回调函数 :收到tf消息时,将会调用函数,完成数据的转化,需要的数据包括:laser_point 、base_point。
*官方历程提供了一个假定的点,首先给这个点的id,以及时间戳赋值
geometry_msgs::PointStampedlaser_point;
laser_point.header.frame_id = "base_laser";
laser_point.header.stamp = ros::Time();
然后是点的(x,y,z)赋值
*转化: listener.transformPoint("base_link", laser_point, base_point);
(1) 生成一个tf::TransformListener对象,该对象会自动订阅ROS中的tf消息,并且管理所有的变换关系数据。
(2) Tf::StampedTransform transform
listener.lookupTransform("/turtle2","/turtle1",ros::Time(0), transform); 转换结果储存在transform中
Time(0):指缓冲区中最近的一次信息
Time::now() :当前时刻的信息,有可能没有,故而需要waitfor transform
https://www.douban.com/note/574961732/
每一个tf的监听者都有一个缓冲区,储存着不同tf发布器的所有坐标转换。当一个发布器发布一个转化,而这个转换进入缓冲区需要一些时间。故而需要添加waitFormTransform
3.Tf 常用工具
1. rosrun rqt_tf_tree rqt_tf_tree
2.tf_echo
rosrun tf tf_echo [reference_frame] [target_frame]
3.rviz
rosrun rviz rviz -d `rospack find turtle_tf`/rviz/turtle_rviz.rviz
4.参数设置
transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
或者
tf::Transform transform;
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
tf::Quaternion q;
q.setRPY(0, 0, msg->theta);
transform.setRotation(q);