ROS学习之TF坐标变换程序

1.静态坐标变换

1.1.坐标变换发布者（broadcaster）

#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"  // 静态坐标发布的头文件
#include "geometry_msgs/TransformStamped.h"   // 发布者要发布的坐标变换消息，内部包含平移向量和旋转四元数
#include "tf2/LinearMath/Quaternion.h" // 四元数可以使用欧拉角来转换得到，这里使用四元数需要包含头文件

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "static_tf_pub");

    tf2_ros::StaticTransformBroadcaster br; // 静态坐标变换发布的对象

    geometry_msgs::TransformStamped ts;

    ts.header.frame_id = "world";
    ts.header.stamp = ros::Time::now();
    ts.child_frame_id = "laser";

    tf2::Quaternion q;
    q.setRPY(0, 0, 0);  // 这里的RPY就是roll pitch yaw的欧拉角，具体什么顺序呢？
    ts.transform.rotation.w = q.getW();
    ts.transform.rotation.x = q.getX();
    ts.transform.rotation.y = q.getY();
    ts.transform.rotation.z = q.getZ();
    
    ts.transform.translation.x = 2.0;
    ts.transform.translation.y = 0.0;
    ts.transform.translation.z = 5.0;

    br.sendTransform(ts);
    // ros::spinOnce();    // 注意这里不能使用spinOnce，否则的话程序执行一次之后直接退出了
    ros::spin();   

    return 0;
}

1.2.坐标变换订阅者（listener）

#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"  // 坐标listener对象所在的头文件
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"  // 使用listener必须包含的头文件
#include "tf2_ros/buffer.h"  // tf2中把订阅得到的坐标变换消息放在buffer中缓存
#include "geometry_msgs/PointStamped.h"  // 要进行点的坐标变换，所以导入点的头文件

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "static_tf_sub");

    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);  // 注意这里必须使用buffer初始化listener

    ros::Rate rate(1);

    while (ros::ok())
    {
        geometry_msgs::PointStamped point;
        point.header.frame_id = "laser";  // 声明当前这个前在是那个坐标下的
        point.header.stamp = ros::Time::now();
        point.point.x = 2.0;
        point.point.y = 2.0;
        point.point.z = 2.0;

		/* 这里使用try catch主要是因为坐标变换的广播是一个比较耗时的操作，可能这里已经订阅了，
		但是坐标发布方那边还没有发布，这个时候直接使用buffer进行坐标变换就会抛出异常，提示
		没有world这个坐标系，实际就是因为此时buffer中还没有缓存到坐标变换的broadcaseter发来
		的消息。所以这里使用try catch，如果出现异常进入catch，那么就打印一个错误消息，然后
		继续执行
		*/
        try
        {
            geometry_msgs::PointStamped point_tf;
            point_tf = buffer.transform(point, "world");  // 得到转换后的坐标，第一次参数是原来的坐标，第二个参数是要转换到那个坐标系下
            ROS_INFO("转换后的坐标是：(%.2f, %.2f, %.2f) , 参考的坐标是：%s",
                        point_tf.point.x, point_tf.point.y, point_tf.point.z,
                        point_tf.header.frame_id.c_str());
        }
        catch(const std::exception& e)
        {
            // std::cerr << e.what() << '\n';
            ROS_INFO("程序异常");
        }
        

        rate.sleep();
        ros::spinOnce();

    }
    

    return 0;
}