ROS学习笔记6 —— TF的使用

1. tf数据类型1

tf/transform_datatypes.h中定义,基本数据类型有:Quaternion, Vector, Point, Pose, Transform

在这里插入图片描述

  • tf::Stamped 对上述数据类型做模板化(除了tf::Transform),并附带元素frame_id_stamp_
  • tf::StampedTransformtf::Transforms的特例,它要求frame_idstamp_(与此转换关联的时间戳) 、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()函数实现,传递StampedTransformgeometry_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

创建的功能包需要依赖于tfroscpprospy,以小乌龟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() 四个参数:
  1. 需要等待变换从坐标系turtle2
  2. 到坐标系turtle1
  3. 在now时间
  4. 超时时间,不要等待超过此最大持续时间

注意:使用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,有六个参数:
  1. 变换从坐标系turtle2
  2. 在now时间
  3. 到turtle1坐标系
  4. 在past时间
  5. 指定不随时间改变的坐标系,这里是world
  6. 变换结果保存的变量
  • waitForTransform()跟lookupTransform()一样有6个相应参数。
    在这里插入图片描述
    在上图中,在world坐标系,tf时间从past到now,past时间tf会计算从turtle1到world坐标系的变换,在now时间,tf计算从world到turtle2坐标系的变换

  1. https://www.ncnynl.com/archives/201702/1305.html ↩︎

  2. https://www.ncnynl.com/archives/201702/1306.html ↩︎

  3. https://www.ncnynl.com/archives/201702/1307.html ↩︎

  4. https://www.ncnynl.com/archives/201702/1308.html ↩︎

  5. https://www.ncnynl.com/archives/201702/1309.html ↩︎

  6. https://www.ncnynl.com/archives/201702/1310.html ↩︎

  7. https://www.ncnynl.com/archives/201702/1311.html ↩︎

  8. https://www.ncnynl.com/archives/201702/1312.html ↩︎

  9. https://www.ncnynl.com/archives/201702/1313.html ↩︎

  10. https://www.ncnynl.com/archives/201702/1314.html ↩︎

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值