buffer.lookupTransform 与buffer.Transform详细解读 结合C++代码

以下是结合C++代码对 `buffer.lookupTransform` 和 `buffer.Transform` 进行详细解读。

### buffer.lookupTransform

`buffer.lookupTransform` 函数的C++接口定义如下：

bool tf2::BufferCore::lookupTransform(
    const std::string& target_frame, const std::string& source_frame,
    const ros::Time& time, StampedTransform& transform) const;

其中，`target_frame` 和 `source_frame` 参数分别表示目标坐标系和源坐标系的名称，`time` 参数表示查询的时间点，`transform` 参数是一个 `StampedTransform` 类型的引用，用于返回查询到的变换矩阵。

具体使用方法如下：

#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "lookup_transform_example");
  
  tf2_ros::Buffer tfBuffer;
  tf2_ros::TransformListener tfListener(tfBuffer);

  // 查询 base_link 到 map 坐标系之间的变换矩阵
  geometry_msgs::TransformStamped transformStamped;
  try{
    transformStamped = tfBuffer.lookupTransform("map", "base_link", ros::Time(0));
  }
  catch (tf2::TransformException &ex) {
    ROS_WARN("%s",ex.what());
    ros::Duration(1.0).sleep();
  }

  ROS_INFO("Translation: x=%f, y=%f, z=%f", transformStamped.transform.translation.x,
           transformStamped.transform.translation.y, transformStamped.transform.translation.z);
  ROS_INFO("Rotation: x=%f, y=%f, z=%f, w=%f", transformStamped.transform.rotation.x,
           transformStamped.transform.rotation.y, transformStamped.transform.rotation.z,
           transformStamped.transform.rotation.w);

  return 0;
}

在以上示例中，我们使用 `tfBuffer` 对象查询 `map` 到 `base_link` 坐标系之间的变换矩阵，并将结果存储在 `transformStamped` 中。需要注意的是，我们指定了查询的时间点为 `ros::Time(0)`，表示查询最近的变换矩阵。

### buffer.Transform

`buffer.Transform` 函数的C++接口定义如下：

bool tf2::BufferCore::transform(
    const geometry_msgs::PointStamped& in, geometry_msgs::PointStamped& out,
    const std::string& target_frame, const ros::Duration& timeout = ros::Duration(0.1)) const;

其中，`in` 参数是一个 `geometry_msgs::PointStamped` 类型的消息，表示要转换的点的位置和所在坐标系的名称。`out` 参数也是一个 `geometry_msgs::PointStamped` 类型的消息，表示转换后的点的位置和所在坐标系的名称。`target_frame` 参数表示目标坐标系的名称。`timeout` 参数表示转换超时时间，默认为 `0.1` 秒。

具体使用方法如下：

#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "transform_example");
  
  tf2_ros::Buffer tfBuffer;
  tf2_ros::TransformListener tfListener(tfBuffer);

  // 创建要转换的点的消息
  geometry_msgs::PointStamped pointStamped;
  pointStamped.header.frame_id = "base_link";
  pointStamped.header.stamp = ros::Time::now();
  pointStamped.point.x = 1.0;
  pointStamped.point.y = 0.0;
  pointStamped.point.z = 0.0;

  // 将点从 base_link 坐标系转换到 map 坐标系
  geometry_msgs::PointStamped transformedPointStamped;
  try{
    tfBuffer.transform(pointStamped, transformedPointStamped, "map");
  }
  catch (tf2::TransformException &ex) {
    ROS_WARN("%s",ex.what());
    ros::Duration(1.0).sleep();
  }

  ROS_INFO("Transformed Point: x=%f, y=%f, z=%f in frame %s", transformedPointStamped.point.x,
           transformedPointStamped.point.y, transformedPointStamped.point.z, transformedPointStamped.header.frame_id.c_str());

  return 0;
}

在以上示例中，我们使用 `tfBuffer` 对象将一个点从 `base_link` 坐标系转换到 `map` 坐标系，并将结果存储在 `transformedPointStamped` 中。需要注意的是，我们在创建要转换的点的消息时，指定了该点所在的坐标系为 `base_link`，并在 `transform` 函数中指定了目标坐标系为 `map`。