tf::transformPose 坐标系变换

最新推荐文章于 2024-06-19 23:15:29 发布

原创最新推荐文章于 2024-06-19 23:15:29 发布 · 5.3k 阅读

23 ·

CC 4.0 BY-SA版权

ROS相关专栏收录该内容

27 篇文章

订阅专栏

本文介绍在使用tf::transformPose进行坐标变换时出现的错误：Lookup would require extrapolation into the future，并提供了解决方案。主要是通过修改时间戳来避免未来数据的预测问题。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

使用tf::transformPose进行坐标变换时候报错：

[ERROR] [1456669076.279804500]: Lookup would require extrapolation into the future.
 Requested time 1456669076.279616253 but the latest data is at time 1456669076.159341977,
  when looking up transform from frame …

原始变换代码类似如下：

#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <ros/ros.h>
void odomCallBack(const nav_msgs::Odometry::ConstPtr& odom)
{
    // transform from "odom" to "map"
    tf::TransformListener listener;

    geometry_msgs::PoseStamped pose_odom;
    pose_odom.header = odom->header;
    pose_odom.pose = odom->pose.pose;

    geometry_msgs::PoseStamped pose_map;

    try{
    //  listener.waitForTransform(...);  //这里省略这个书写，实际中可能会用到
        listener.transformPose("map", pose_odom, pose_map);
    }
    catch( tf::TransformException ex)
    {
        ROS_WARN("transfrom exception : %s",ex.what());
        return;
    }

    ROS_INFO_STREAM(pose_map);
}

报错原因是由于tf的会把监听的内容存放到一个缓存中，然后再读取相关的内容，而这个过程可能会有几毫秒的延迟，也就是，tf的监听器并不能监听到“现在”的变换，
更改这个错误需要改变时间戳，不能使用ros::Time::now(),或者传感器给定的默认时间戳，需要使用ros::Time()替换原来的时间戳。
增加下面一行：

pose_odom.header.stamp = ros::Time();

更改后完整程序为：

#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <ros/ros.h>
void odomCallBack(const nav_msgs::Odometry::ConstPtr& odom)
{
    // transform from "odom" to "map"
    tf::TransformListener listener;

    geometry_msgs::PoseStamped pose_odom;
    pose_odom.header = odom->header;
    pose_odom.header.stamp = ros::Time();  //增加了这行
    pose_odom.pose = odom->pose.pose;

    geometry_msgs::PoseStamped pose_map;

    try{
    //  listener.waitForTransform(...);  //这里省略这个书写，实际中可能会用到
        listener.transformPose("map", pose_odom, pose_map);
    }
    catch( tf::TransformException ex)
    {
        ROS_WARN("transfrom exception : %s",ex.what());
        return;
    }

    ROS_INFO_STREAM(pose_map);
}