TF常用的命令
rosrun rqt_tf_tree rqt_tf_tree //在ROS里面查看tf变换关系
roswtf //roswtf可以检查tf的配置并发现常见问题
rosrun rivz rivz //同时可以配合rviz直观查看各个坐标系之间的关系
rosrun tf tf_echo /world /turtle1 //查看turtle1在world在world中的坐标
https://blog.csdn.net/qq_39779233/article/details/108215144
TF的原理
TF库的目的是实现系统中任一个点在所有坐标系之间的坐标变换。也就是说,只要给定一个坐标系下的一个点的坐标,就能获得这个点在其他任意坐标系的坐标。
在TF树中具有父子关系的坐标系是相邻的,用带箭头的线连接起来。在TF树中用箭头表示这种父子关系。上图所表示的TF树中world坐标系是父坐标系,turtle1和turtle2都是world的子坐标系。
以官网的TF变换为例作一部分介绍,图中一个是机器人本体 base_link 与激光雷达 base_laser 的TF变换,红色的为base_link,蓝色的为base_laser。
如果以base_link作为基准,那么base_laser相对于base_link的水平方向的距离为0.1m,竖直方向的距离为0.2m.因此,从base_laser到base_link的坐标转换关系应该为(x:0.1m,y:0.0m, z:0.2m),相反的转换即是(x:-0.1m,y:0.0m,z:-0.2m)。
利用tf来管理这种关系,需要把他们添加到转换树 transform tree 中。一方面,转换树中的每一个节点都对应着一类坐标系,节点之间的连线即是两个坐标相互转换的一种表示,一种从当前节点到子节点的转换表示。Tf利用树结构的方式,保证了两个坐标系之间的只存在单一的转换,同时假设节点之间的连线指向是从parent到child。
我们需要创建两个节点,一个“base_link”,一个是“base_laser”。为了定义两者的关系,首先,我们需要决定谁是parent,谁是child。时刻记得,由于tf假设所有的转换都是从parent到child的,因此谁是parent是有差别的。我们选择“base_link”坐标系作为parent,其他的传感器等,都是作为“器件”被添加进robot的,对于“base_link”和“base_laser”他们来说,是最适合的。这就意味着转换关系的表达式应该是(x:0.1m,y0.0m,z:0.2m)。关系的建立,在收到“base_laser”的数据到“base_link”的转换过程,就可以是简单的调用tf库即可完成。我们的机器人,就可以利用这些信息,在“base_link”坐标系中,就可以推理出传感器扫描出的数据,并可安全的规划路径和避障等工作。【参考】
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>//包含tf下面的广播tf变换头文件
int main(int argc, char** argv){
ros::init(argc, argv, "robot_tf_publisher");//初始化robot_tf_publisher节点
ros::NodeHandle n; //在robot_tf_publisher节点下面新建一个节点控制器n
ros::Rate r(100); //设置节点控制器n的发布频率为100
tf::TransformBroadcaster broadcaster; //创建tf变换的一个对象(tf类下面的广播的对象,如果不懂,加强c++学习)
while(n.ok()){ //循环判断节点控制器是否计满100,若是则执行
broadcaster.sendTransform( //tf创建的对象发布消息
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")); //base_laser转换到base_link
//发布的消息有5个元素,
//第一个元素tf::Quaternion为旋转变换,就是说base_laser在转换到base_link的时候是否需要进行一定的旋转,
//第二个元素,tf::Vector3为x,y,z三轴的位移变换,
//第三个元素为变换的时间(因为变换是根据一定时间执行一次),
//第四个元素为转换的基准目标,也就是转换后到这个坐标系,
//第五个元素为待转换的坐标系
n.sleep();//没有操作的时候,节点控制器休眠
}
}
广播TF:
引用ROS21讲里面的例程:
/**
* 该例程产生tf数据,并计算、发布turtle2的速度指令
* 运行流程:
* roscore
* rosrun turtlesim turtlesim_node
* rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_broadcaster /turtle1
* rosrun learning_tf turtle_tf_broadcaster __name:=turtle2_tf_broadcaster /turtle2
* rosrun learning_tf turtle_tf_listener
* rosrun turtlesim turtle_teleop_key
*/
#include <ros/ros.h>
#include <tf/transform_broadcaster.h> //包含tf下面的广播tf变换头文件
#include <turtlesim/Pose.h> //海龟的位姿
std::string turtle_name;
void poseCallback(const turtlesim::PoseConstPtr& msg){ //传进来的是全局坐标系下海龟的位姿信息
static tf::TransformBroadcaster br;
tf::Transform transform; // 初始化tf数据 机器人导论里面一个4*4的转换矩阵,保存了平移关系和旋转关系
// setOrigin()函数的参数类型需要为tf::Vector3类型
// 假设是要发布一个子坐标系为”turtle1”父坐标系为“world”,那么其中(msg->x,msg->y,0.0)是指“turtle1”的坐标原点在“world”坐标系下的坐标
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) ); //设置平移参数
tf::Quaternion q; //设置旋转,四元数
q.setRPY(0, 0, msg->theta); //完成了两个坐标系之间姿态变换的关系
transform.setRotation(q); //通过setRotation设置旋转关系到transform里面
// 广播world与海龟坐标系之间的tf数据 ros::Time::now():添加时间戳,tf默认是有一个时间的概念的,默认保存10s之内
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}
// char** argv:通过这里的输入去判断是turtle1还是turtle2, 任意的节点名在ROS环境当中只能有一个
// 假如这个程序执行两遍的话节点名可能会冲突, 所以会想办法让节点的名字换一个,这样程序就可以执行两遍了,这叫重映射
int main(int argc, char** argv){
ros::init(argc, argv, "my_tf_broadcaster");
if (argc != 2){ // 输入参数作为海龟的名字
ROS_ERROR("need turtle name as argument");
return -1;
}
turtle_name = argv[1];
ros::NodeHandle node;
//这个subscribe会订阅海龟仿真器里面不断发布的海龟的位置消息,在turtlesim中默认发布的是turtle1 /pose,所以他要这样定义
ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
// 循环等待回调函数,一旦有海龟位置发布的话,会跳转到这个回调函数里面
ros::spin();
return 0;
};
关于这两个:
rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_broadcaster /turtle1
rosrun learning_tf turtle_tf_broadcaster __name:=turtle2_tf_broadcaster /turtle2
先运行第一个,第一个是用来发布turtle1跟world之间的坐标关系,就运行learning_tf中的broadcaster,当然这个broadcaster的main函数会读取我们的输入参数,输入参数就是/turtle1,/turtle1就是海龟的名字,中间的__name:=turtle1_tf_broadcaster 就是ROS里面的重映射的机制,在程序里面已经默认给节点初始化的时候有一个命名了,但是同样的程序运行两次,所以给他进行重新命名,提高代码复用性。
__name:=turtle1_tf_broadcaster,turtle1_tf_broadcaster这个名字会自动取代程序中定义的my_tf_broadcaster。
监听TF:
/**
* 该例程监听tf数据,并计算、发布turtle2的速度指令 为了测试使用
* 运行流程:
* roscore
* rosrun turtlesim turtlesim_node
* rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_broadcaster /turtle1
* rosrun learning_tf turtle_tf_broadcaster __name:=turtle2_tf_broadcaster /turtle2
* rosrun learning_tf turtle_tf_listener
* rosrun turtlesim turtle_teleop_key
*/
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>
int main(int argc, char** argv){
ros::init(argc, argv, "my_tf_listener");
ros::NodeHandle node;
ros::service::waitForService("/spawn");// 请求产生turtle2
ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");
turtlesim::Spawn srv;
add_turtle.call(srv);
ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);
tf::TransformListener listener;
ros::Rate rate(10.0);
while (node.ok()){
// 获取turtle1与turtle2坐标系之间的tf数据
tf::StampedTransform transform;
try{
listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0)); //由turtle2转换到turtle1
listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform); //由turtle2转换到turtle1
}
catch (tf::TransformException &ex){
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
// 根据turtle1与turtle2坐标系之间的位置关系,发布turtle2的速度控制指令
geometry_msgs::Twist vel_msg;
vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(), transform.getOrigin().x());
vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + pow(transform.getOrigin().y(), 2)); //C 库函数 double pow(double x, double y) 返回 x 的 y 次幂
turtle_vel.publish(vel_msg);
rate.sleep();
}
return 0;
};
注意:【参考】
A.由于tf的会把监听的内容存放到一个缓存中,然后再读取相关的内容,而这个过程可能会有几毫秒的延迟,也就是,tf的监听器并不能监听到“现在”的变换,所以如果不使用try,catch函数会导致报错,导致程序崩溃。
B.不可以把ros::Time(0)改成ros::time::now(),因为监听做不到实时,会有几毫秒的延迟。ros::Time(0)指最近时刻存储的数据,ros::time::now()则指当下,如果非要使用ros::time::now,则需要结合waitForTransform()使用-请参考:链接。
C.waitForTransform() 四个参数:
1.需要等待变换从坐标系turtle2
2.到坐标系turtle1
3.在now时间
4.超时时间,不要等待超过此最大持续时间
查看节点之间的关系:
rosrun rqt_graph rqt_graph
查看TF树:
rosrun rqt_tf_tree rqt_tf_tree
配合rviz查看TF:
rosrun rivz rivz