ROS代码经验系列-- tf进行位置查询变换

最新推荐文章于 2025-06-06 14:41:26 发布

转载最新推荐文章于 2025-06-06 14:41:26 发布 · 757 阅读

编程同时被 2 个专栏收录

29 篇文章

订阅专栏

ROS

10 篇文章

订阅专栏

本文详细介绍了ROS中tf变换的应用，包括如何获取机器人在特定时刻的位置、如何转换相对坐标系中的点到绝对坐标系，以及如何处理激光扫描仪角度与机器人坐标系之间的转换。

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

include文件：

</pre><pre name="code" class="cpp">// For transform support
#include "tf/transform_broadcaster.h"
#include "tf/transform_listener.h"
#include "tf/message_filter.h"
#include "tf/tf.h"
#include "message_filters/subscriber.h"

某时刻机器人在地图上的位置：

当机器人在移动过程中，tf会不断接收 base_link->odom 的位置关系信息，这些信息是根据时间不断变化并被记录下来的。当其它节点需要获取某个时间点上的 base_link的位置时就可以通过下面的方法查询：

x, y, yaw 就是base_link 在t 时刻在地图上的位置：

bool getOdomPose(tf::Stamped<tf::Pose>& odom_pose,
                      double& x, double& y, double& yaw,
                      const ros::Time& t, const std::string& base_link)
{
  // Get the robot's pose
  tf::Stamped<tf::Pose> ident (tf::Transform(tf::createIdentityQuaternion(),
                                           tf::Vector3(0,0,0)), t, base_link );
  try
  {
    tf_ = new tf::TransformListener();

    tf_->transformPose(odom_frame_id_, ident, odom_pose);
  }
  catch(tf::TransformException e)
  {
    ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
    return false;
  }
  x = odom_pose.getOrigin().x();
  y = odom_pose.getOrigin().y();
  double pitch,roll;
  odom_pose.getBasis().getEulerYPR(yaw, pitch, roll);

  return true;
}

机器人某个位置相对map的位置关系：

机器人是矩形，四个角儿相对中心的位置已知，获取四个角相对map的位置

tf::Stamped<tf::Pose> corner1(
    tf::Pose(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.30, -0.45, 0.0)),
    ros::Time(0), "base_link");
tf::Stamped<tf::Pose> corner2(
    tf::Pose(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.30, 0.45, 0.0)),
    ros::Time(0), "base_link");
tf::Stamped<tf::Pose> corner3(
    tf::Pose(tf::Quaternion(0, 0, 0, 1), tf::Vector3(-0.30, -0.45, 0.0)), 
    ros::Time(0), "base_link");
tf::Stamped<tf::Pose> corner4(
    tf::Pose(tf::Quaternion(0, 0, 0, 1), tf::Vector3(-0.30, 0.45, 0.0)), 
    ros::Time(0), "base_link");
transform_listener = new tf::TransformListener();
tf::Stamped<tf::Pose> transformed_corner_1;
transform_listener.transformPose("map", corner_1, transformed_corner_1);
tf::Stamped<tf::Pose> transformed_corner_2;
transform_listener.transformPose("map", corner_2, transformed_corner_2);
tf::Stamped<tf::Pose> transformed_corner_3;
transform_listener.transformPose("map", corner_3, transformed_corner_3);
tf::Stamped<tf::Pose> transformed_corner_1;
transform_listener.transformPose("map", corner_4, transformed_corner_4);

随机器人移动点在t+1时刻的位置

已知 t 时刻的位置是 pose_old，求t+1 时刻的位置 pose_new

  tf_ = new tf::TransformListener();
  tf::StampedTransform tx_odom;
  try
  {
    tf_->lookupTransform(base_frame_id_, ros::Time::now(),
                         base_frame_id_, msg.header.stamp,
                         global_frame_id_, tx_odom);
  }
  catch(tf::TransformException e)
  {
    ROS_WARN("Failed to transform initial pose in time (%s)", e.what());
    tx_odom.setIdentity();
  }
  
  tf::Pose pose_old, pose_new;
  tf::poseMsgToTF(msg.pose.pose, pose_old);
  pose_new = tx_odom.inverse() * pose_old;


  // Transform into the global frame
  ROS_INFO("Setting pose (%.6f): %.3f %.3f %.3f",
           ros::Time::now().toSec(),
           pose_new.getOrigin().x(),
           pose_new.getOrigin().y(),
           getYaw(pose_new));

相对角度的转换Quaternion

当base_link代表机器人时，激光扫描仪laser_scan安装的角度与base_link不平行，即激光数据的零度不对应机器人的正前方零度。已知 laser_scan->angle_min 和 laser_scan->angle_increment 为激光数据信息，转换角度到base_link的位置代码如下，该算法可以考虑到激光器上下颠倒安装的情况导致angle_increment为负：

    tf::Quaternion q;
    q.setRPY(0.0, 0.0, laser_scan->angle_min);
    tf::Stamped<tf::Quaternion> min_q(q, laser_scan->header.stamp,
                                      laser_scan->header.frame_id);
    q.setRPY(0.0, 0.0, laser_scan->angle_min + laser_scan->angle_increment);
    tf::Stamped<tf::Quaternion> inc_q(q, laser_scan->header.stamp,
                                      laser_scan->header.frame_id);
    try
    {
      tf_->transformQuaternion(base_frame_id_, min_q, min_q);
      tf_->transformQuaternion(base_frame_id_, inc_q, inc_q);
    }
    catch(tf::TransformException& e)
    {
      ROS_WARN("Unable to transform min/max laser angles into base frame: %s",
               e.what());
      return;
    }

    double angle_min = tf::getYaw(min_q);
    double angle_increment = tf::getYaw(inc_q) - angle_min; //考虑到了激光器上下颠倒安装的情况导致为负数