tf/tf2

最新推荐文章于 2025-07-01 09:22:57 发布

原创最新推荐文章于 2025-07-01 09:22:57 发布 · 1.7k 阅读

0 ·

CC 4.0 BY-SA版权

ROS 专栏收录该内容

83 篇文章

订阅专栏

本文介绍如何使用ROS中的tf2进行坐标系转换，并详细解释了如何通过差分计算来获取机器人移动的速度指令。此外，还展示了如何利用tf库进行坐标变换，以及如何计算两个位置之间的角度差。

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

矩阵换算

#include "ros/ros.h"
#include "geometry_msgs/PointStamped.h"

#include "tf2_ros/transform_listener.h"
#include "tf2_ros/message_filter.h"
#include "message_filters/subscriber.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"

//tf2 转变坐标系
tf2_ros::Buffer buffer_;
tf2_ros::TransformListener tf2_;

geometry_msgs::PointStamped point_out;
buffer_.transform(pose_in, pose_out, target_frame_);


// 方式2
  tf2::doTransform(pose_in, pose_out, geometry_msgs::TransformStamped tf_plan_to_global);


//tf
try {
    listener_.waitForTransform(odom_frame_, "base_link", ros::Time(0), ros::Duration(0.1));
    listener_.transformPose("base_link", pose_in_odom, pose_in_base);
} catch (tf::TransformException const& ex) {
  ROS_WARN("BaseController, Couldn't transform dock pose to tracking frame");
  return false;
}

diff2D, 获得输出速度PoseFollower.cpp

// usage
// diff2D(target_pose, robot_pose);


  geometry_msgs::Twist diff2D(const tf::Pose& pose1, const tf::Pose& pose2)
  {
    geometry_msgs::Twist res;
    tf::Pose diff = pose2.inverse() * pose1;
    res.linear.x = diff.getOrigin().x();
    res.linear.y = diff.getOrigin().y();
    res.angular.z = tf::getYaw(diff.getRotation());

    if(holonomic_ || (fabs(res.linear.x) <= tolerance_trans_ && fabs(res.linear.y) <= tolerance_trans_))
      return res;

    //in the case that we're not rotating to our goal position and we have a non-holonomic robot
    //we'll need to command a rotational velocity that will help us reach our desired heading
    
    //we want to compute a goal based on the heading difference between our pose and the target
    double yaw_diff = headingDiff(pose1.getOrigin().x(), pose1.getOrigin().y(), 
        pose2.getOrigin().x(), pose2.getOrigin().y(), tf::getYaw(pose2.getRotation()));


    //compute the desired quaterion
    tf::Quaternion rot_diff = tf::createQuaternionFromYaw(yaw_diff);
    tf::Quaternion rot = pose2.getRotation() * rot_diff;
    tf::Pose new_pose = pose1;
    new_pose.setRotation(rot);

    diff = pose2.inverse() * new_pose;
    res.linear.x = diff.getOrigin().x();
    res.linear.y = diff.getOrigin().y();
    res.angular.z = tf::getYaw(diff.getRotation());
    return res;
  }

// 计算yawdiff， PoseFollower.cpp
// x,y 是 target， pt 是current pose;  这些点都是在odom 下讲的
 double headingDiff(double x, double y, double pt_x, double pt_y, double heading)
  {
    double v1_x = x - pt_x;
    double v1_y = y - pt_y;
    double v2_x = cos(heading);
    double v2_y = sin(heading);

    double perp_dot = v1_x * v2_y - v1_y * v2_x;   // 叉乘的意思（法线）， |a| * |b| * sin
    double dot = v1_x * v2_x + v1_y * v2_y;        // 点乘的意义(投影)， |a| × |b| × cos

    //get the signed angle
    double vector_angle = atan2(perp_dot, dot);

    return -1.0 * vector_angle;
  }

点乘叉乘