ROS——TF

在ROS中，tf是一个用于处理坐标变换的包，它提供了一种在不同坐标系之间进行转换的方法。使用tf可以很方便地实现机器人姿态的表示和控制，以及实现基于坐标系的导航和路径规划等功能。在C++中使用tf，需要包含<tf/transform_listener.h>头文件，并创建一个tf::TransformListener对象来监听坐标变换。

下面是一个使用tf在C++中实现坐标变换的简单示例：

#include <ros/ros.h>
#include <tf/transform_listener.h>

int main(int argc, char** argv){
    ros::init(argc, argv, "tf_listener");

    ros::NodeHandle node;
    tf::TransformListener listener;

    ros::Rate rate(10.0);
    while (node.ok()){
        tf::StampedTransform transform;
        try{
            listener.lookupTransform("/base_link", "/map", ros::Time(0), transform);
        }
        catch (tf::TransformException ex){
            ROS_ERROR("%s",ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }

        double x = transform.getOrigin().x();
        double y = transform.getOrigin().y();
        double yaw = tf::getYaw(transform.getRotation());

        ROS_INFO("Current pose: x=%f, y=%f, yaw=%f", x, y, yaw);

        rate.sleep();
    }
    return 0;
}

在这个例子中，我们创建了一个tf::TransformListener对象来监听"/base_link"坐标系相对于"/map"坐标系的变换，并输出当前的机器人姿态信息。通过调用lookupTransform()函数，我们可以获取当前时刻"/base_link"相对于"/map"的坐标变换信息，并将其存储在tf::StampedTransform对象中。然后，我们可以通过getOrigin()和getRotation()函数获取位置和姿态信息，并使用getYaw()函数将四元数转换为欧拉角yaw。最后，我们使用ROS_INFO()函数输出当前的机器人姿态信息。循环周期可以通过调用rate.sleep()函数来控制。

ros::Time(0)是一个用于获取当前时刻的ROS时间戳的方法之一。在ROS中，时间戳通常用于对机器人的传感器数据和控制指令进行时间同步和对齐。ROS时间戳是一个由两个32位整数表示的时间戳，其中一个整数表示时间戳的秒数，另一个整数表示时间戳的纳秒数。

在上述示例代码中，ros::Time(0)被传递给listener.lookupTransform()函数中的时间参数。这表示我们想要获取当前时刻（最新的）"/base_link"坐标系相对于"/map"坐标系的变换信息。也就是说，我们想要获取"/base_link"坐标系相对于"/map"坐标系的最新变换，以确保位置和姿态信息的准确性。

ex.what()是一个C++中的异常处理方法，用于获取异常对象的错误信息。在C++中，当程序出现错误或异常时，可以通过抛出异常来通知程序出现了问题，并且可以通过异常处理程序来捕获和处理异常。在捕获到异常后，可以使用ex.what()方法获取异常的错误信息，以便于程序的调试和修复。

在ROS中，也可以使用C++中的异常处理机制来处理程序出现的异常。在上述代码中，我们可以看到try-catch代码块，其中catch语句用于捕获ROS异常，并且使用ex.what()方法获取异常的错误信息，以便于程序调试和修复。