文章目录
1. tf数据类型1
在tf/transform_datatypes.h
中定义,基本数据类型有:Quaternion
, Vector
, Point
, Pose
, Transform
tf::Stamped
对上述数据类型做模板化(除了tf::Transform
),并附带元素frame_id_
和stamp_
tf::StampedTransform
是tf::Transforms
的特例,它要求frame_id
、stamp_
(与此转换关联的时间戳) 、child_frame_id
- 辅助函数:
tf::Quaternion createIdentityQuaternion()
,返回四元数句柄tf::Quaternion createQuaternionFromRPY(double roll,double pitch,double yaw)
,返回从固定轴的Roll, Pitch and Yaw(滚动,俯仰和偏转)构造的tf::Quaternion四元数geometry_msgs::Quaternion createQuaternionMsgFromRollPitchYaw(double roll,double pitch,double yaw)
,返回从固定轴的Roll, Pitch and Yaw(滚动,俯仰和偏转)构造的geometry_msgs::Quaternion四元数
2. tf坐标变换2
Frame是坐标系统,在ROS里都是以3D形式存在,右手原则,X向前,Y向左,Z向上
Points在坐标系以tf::Point形式表达,等同与bullet类型的btVector3
在坐标系W里,坐标的点p,可以用:Wp
3. tf广播变换3
-
在节点里广播变换,推荐使用
tf::TransformBroadcaster
(广播器). -
发送变换通过调用
sendTransform()
函数实现,传递StampedTransform
或geometry_msgs::TransformStamped
为参数
void sendTransform(const StampedTransform & transform);
void sendTransform(const geometry_msgs::TransformStamped & transform);
4. 使用已发布的变换4
- 通过调用
tf::TransformListener
类(监听器)来处理变换,它继承自tf::Transformer
核心方法:
-
(1)构造函数
TransformListener(const ros::NodeHandle &nh, ros::Duration max_cache_time=ros::Duration(DEFAULT_CACHE_TIME), bool spin_thread=true) TransformListener(ros::Duration max_cache_time=ros::Duration(DEFAULT_CACHE_TIME), bool spin_thread=true)
-
(2)辅助方法
std::string tf::TransformListener::resolve (const std::string &frame_id)
结合tf_prefix获取解析的frame_id
-
(3)canTransform()函数
返回bool ,判断能否实现变换。不会抛出异常,如果出错,会返回error_msg的内容。-
基本API:
bool tf::TransformListener::canTransform (const std::string &target_frame, const std::string &source_frame, const ros::Time &time, std::string *error_msg=NULL) const // 检查在时间time,source_frame能否变换到target_frame
-
高级API:
bool tf::TransformListener::canTransform (const std::string &target_frame, const ros::Time &target_time, const std::string &source_frame, const ros::Time &source_time, const std::string &fixed_frame, std::string *error_msg=NULL) const // 检查在时间source_time,source_frame能否变换到fixed_frame,那么再实现在target_time变换到target_frame
-
-
(4)waitForTransform() 函数
返回bool值,评估变换是否有效-
基本API:
bool tf::TransformListener::waitForTransform (const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const // 检查在时间time, source_frame能否变换到target_frame // 它将休眠并重试每个polling_duration,直到超时的持续时间已经过去。 它不会抛出异常。 在一个错误的情况下,如果你传递一个非NULL字符串指针,它会填充字符串error_msg。 (注意:这需要大量的资源来生成错误消息。)
-
高级API:
bool tf::TransformListener::waitForTransform (const std::string &target_frame, const ros::Time &target_time, const std::string &source_frame, const ros::Time &source_time, const std::string &fixed_frame, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const // 测试在source_time时间,source_frame能否变换到fixed_frame,那么在target_time,变换到target_frame。
-
-
(5)lookupTransform() 函数
-
返回两个坐标系的变换
-
这个方法是 tf库的核心方法。大部分transform的方法都是终端用户使用,而这个方法设计在transform()方法内使用的
-
返回的变换的方向:将source_frame变换到target_frame(注意:此处创客智造描述有误)
-
这个方法会抛出TF的异常
-
基本API:
void tf::TransformListener::lookupTransform (const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const // 在时间time上使用从source_frame到target_frame的变换填充transform
-
高级API:
void tf::TransformListener::lookupTransform (const std::string &target_frame, const ros::Time &target_time, const std::string &source_frame, const ros::Time &source_time, const std::string &fixed_frame, StampedTransform &transform) const // 在source_time使用从source_frame到fixed_frame的变换填充transform,在target_time从fixed_frame到target_frame的链接变换。
-
-
transformDATA 方法
tf::TransformListener
类的主要目的是在坐标系间进行变换数据。-
基本API:
void tf::TransformListener::transformDATATYPE (const std::string &target_frame, const geometry_msgs::DATATYPEStamped &stamped_in, geometry_msgs::DATATYPEStamped &stamped_out) const // 将数据stamped_in转换为target_frame,使用标记的数据类型中的frame_id和stamp作为源。
-
高级API:
void tf::TransformListener::transformDATATYPE (const std::string &target_frame, const ros::Time &target_time, const geometry_msgs::DATATYPEStamped &pin, const std::string &fixed_frame, geometry_msgs::DATATYPEStamped &pout) const // 将数据stamped_in转换为fixed_frame。 使用标记数据类型中的frame_id和stamp作为源。 然后在target_time从fixed_frame变换到target_frame。
-
5. 异常5
-
roscpp所有的异常都继承基类
ros::Exception
-
ros::InvalidNodeNameException
,当无效的基本名称传递到 ros::init()就会抛出异常,通常是带了/
. -
ros::InvalidNameException
,如果名称以~
开始传递到任何的NodeHandle方法都会抛出异常;无效名传递到roscpp函数都会抛出异常。 -
所有在TF里异常继承自
tf::TransformException
,它继承自std::runtime_error
。 -
异常类:
tf::ConnectivityException
作用:如果由于两个坐标系ID不在同一个连接的树中而无法完成请求,则抛出。 -
异常类:
tf::ExtrapolationException
作用:如果请求的坐标系id之间存在连接,但一个或多个变换已过期,则抛出。 -
异常类:
tf::InvalidArgument
作用:如果参数无效则抛出。 最常见的情况是非规范化的四元数。 -
异常类:
tf::LookupException
作用:如果引用了未发布的坐标系ID,则抛出。
6. 编写tf广播6
创建的功能包需要依赖于tf
、roscpp
、rospy
,以小乌龟Demo为例,还需要添加依赖turtlesim
// tf包提供了TransformBroadcaster的实现,以帮助使发布变换的任务更容易
// 要使用TransformBroadcaster,我们需要包含tf/transform_broadcaster.h头文件
#include<ros/ros.h>
#include<tf/transform_broadcaster.h>
#include<turtlesim/Pose.h>
using namespace std;
std::string turtle_name;
void poseCallback(const turtlesim::PoseConstPtr& msg)
{
// TF广播器
// 创建一个TransformBroadcaster对象,我们稍后将使用它通过线路发送转换
static tf::TransformBroadcaster br;
// 创建一个Transform对象,并将信息从2D乌龟姿势复制到3D变换中
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);
// 发布坐标变换
// 使用TransformBroadcaster发送转换需要四个参数;首先传递转换对象,采用ros::Time::now()来给发布的变换一个时间戳;然后需要传递创建的链接的父框架名称,此处为“world”;最后需要传递正在创建的链接的子框架名称,此处为乌龟本身名称
br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"world",turtle_name));
}
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;
ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
ros::spin();
return 0;
}
注意:sendTransform和StampedTransform具有父对象和子对象的相反顺序。
使用tf_echo
工具检查位姿是否广播到tf:
rosrun tf tf_echo /world /turtle1
7. 编写tf监听器7
// tf包提供了TransformListener的实现,以帮助使接收变换的任务更容易
// 要使用TransformListener,我们需要包括tf/transform_listener.h头文件
#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");
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);
// 创建一个TransformListener对象,一旦监听器被创建,它开始接收tf转换,并缓冲它们长达10秒;TransformListener对象应该被限定为持久化,否则它的缓存将无法填充,并且几乎每个查询都将失败;一个常见的方法是使TransformListener对象成为一个类的成员变量
tf::TransformListener listener;
ros::Rate rate(10.0);
while (node.ok()){
tf::StampedTransform transform;
try{
// 查询监听器进行特定的转换,从/turtle2坐标系开始变换到/turtle1坐标系,变换的时间,提供ros::Time(0)即会给出最近的可用的变换,结果存放的变换对象
listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
}
catch (tf::TransformException &ex) {
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
// 变换用于计算龟的新的线性和角速度,基于它与龟的距离和角度。
// 新的速度发布在话题"turtle2/cmd_vel"中,turtlesim将使用它来更新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));
turtle_vel.publish(vel_msg);
rate.sleep();
}
return 0;
};
8. 增加坐标系8
- tf建立坐标系的树结构; 它不允许在坐标系结构中存在闭环
- 一个坐标系只有一个父系,但它可以有多个子系
- 如果我们要向tf添加一个新坐标系,三个现有坐标系中的一个需要是父系,新坐标系将成为子系
以乌龟为例子,添加一个新坐标系carrot1到turtle1
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
int main(int argc, char** argv){
ros::init(argc, argv, "my_tf_broadcaster");
ros::NodeHandle node;
tf::TransformBroadcaster br;
tf::Transform transform;
ros::Rate rate(10.0);
while (node.ok()){
创建transform,从父系turtle1到子系carrot1,carrot1离turtle1 两米远
transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));
rate.sleep();
}
return 0;
};
9. 深入Time和TF9
- 坐标系树随时间变化而变化,tf存储每个变换的时间快照(默认情况下最多为10秒)
- 使用
lookupTransform()
函数来获取该tf树中最新的可用变换,使用now()来记录当前时间的变换
值得注意的是,如果只使用
lookupTransform()
在运行时程序会报错,这是因为 每个监听器有一个缓冲区,它存储来自不同tf广播者的所有坐标变换。 当广播者发出变换时,变换进入缓冲区之前需要一些时间(通常是几个毫秒),所以在时间“now”请求坐标系变换时等待几毫秒再获得该信息便解决了此问题。
try{
ros::Time now = ros::Time::now();
listener.waitForTransform("/turtle2", "/turtle1", now, ros::Duration(3.0));
listener.lookupTransform("/turtle2", "/turtle1",now, transform);
- waitForTransform() 四个参数:
- 需要等待变换从坐标系turtle2
- 到坐标系turtle1
- 在now时间
- 超时时间,不要等待超过此最大持续时间
注意:使用ros::Time::now()是为了这个例子。通常这将是希望被转换的数据的时间戳。waitForTransform()实际上会阻塞直到两个海龟之间的变换可用(这通常需要几毫秒)或者如果变换不可用,直到达到超时。
对于真实的tf用例,使用Time(0)通常是完全正常的。
10. 时间穿梭(Time travel)10
try{
ros::Time now = ros::Time::now();
listener.waitForTransform("/turtle2", "/turtle1",
now, ros::Duration(1.0));
listener.lookupTransform("/turtle2", "/turtle1",
now, transform);
同样以小乌龟Demo为例,现在,不是让turtle2去到turtle1当前时间的地方,而让turtle2去turtle1是5秒前的地方:
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);
相对于/turtle2目前的位置,/turtle1 5秒前的姿势是什么?
回答该问题需要依赖于高级API,示例代码如下:
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);
- 这个lookupTransform()API,有六个参数:
- 变换从坐标系turtle2
- 在now时间
- 到turtle1坐标系
- 在past时间
- 指定不随时间改变的坐标系,这里是world
- 变换结果保存的变量
- waitForTransform()跟lookupTransform()一样有6个相应参数。
在上图中,在world坐标系,tf时间从past到now,past时间tf会计算从turtle1到world坐标系的变换,在now时间,tf计算从world到turtle2坐标系的变换