ROS 1.0 学习笔记（8）tf记录

最新推荐文章于 2024-05-30 11:05:52 发布

碎步の流年

最新推荐文章于 2024-05-30 11:05:52 发布

阅读量207

点赞数

分类专栏： ROS 学习笔记

本文链接：https://blog.csdn.net/qq_24649627/article/details/104656420

版权

ROS 学习笔记专栏收录该内容

8 篇文章 3 订阅

订阅专栏

年前在另一片文章中，详细的介绍了，四元素，欧拉角，轴-角等相互之间的转换，不论何种方式，最终的目的都是一点，实现一个坐标系到另一个坐标系的变换。
这里把ROS1.0中的tf单独拿出来学习下，首先奉上官网的资料
官网上举了两个简单的栗子，下面上代码，代码的编译以及其他环境配置去官网查询就好，这里就不再写一遍了

turtle_tf_broadcaster.cpp

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr& msg)
{
    static tf::TransformBroadcaster br;

    tf::Transform transform;
    transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );

    tf::Quaternion q;
    q.setRPY(0, 0, msg->theta);
    transform.setRotation(q);

    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "my_tf_broadcaster");
    if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};
    turtle_name = argv[1];

    ros::NodeHandle node;
    ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

    ros::spin();
    return 0;
};

tf::TransformBroadcaster 类似于 publish的作用，把需要广播出去的坐标系之间的关系发送出去，既然要发出去的是不同坐标系之间的转换方式，那么就需要确定不同坐标系之间的位置以及旋转信息了，这里选取 world 为父坐标系，so 先设置 turtle 的位置与 world 之间的平移量 transform.setOrigin() ，然后设置欧拉角的旋转信息 transform.setRotation(q)，恩，最后就是把 turtle 和 world 之间的转换发出去，由于有两个 turtle ，因此echo是看到的为，这段代码，直接把两个turtle的坐标转换信息都处理了。
在这里插入图片描述

turtle_tf_listener.cpp

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>

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

    ros::NodeHandle node;

    ros::service::waitForService("spawn");
    ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("spawn");
    turtlesim::Spawn srv;
    add_turtle.call(srv);

    ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);

    tf::TransformListener listener;

    ros::Rate rate(10.0);
    while (node.ok())
    {
        tf::StampedTransform transform;

        try 
        {
        	// ros::Time(0) means "the latest transform" in the buffer
            listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(10.0) );
            listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
            
            // listener.waitForTransform("/turtle2", "/carrot1", ros::Time(0), ros::Duration(10.0) );
            // listener.lookupTransform("/turtle2", "/carrot1", ros::Time(0), transform);            
        } 
        catch (tf::TransformException ex) 
        {
            ROS_ERROR("%s",ex.what());
        }

        // turtlesim::Velocity vel_msg;
        // vel_msg.angular = 4.0 * atan2(transform.getOrigin().y(), transform.getOrigin().x());                                    
        // vel_msg.linear = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + pow(transform.getOrigin().y(), 2));
        geometry_msgs::Twist vel_msg;
        vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(), transform.getOrigin().x());                            
        vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + pow(transform.getOrigin().y(), 2));
                                    
        turtle_vel.publish(vel_msg);

        rate.sleep();
    }

    return 0;
};

第二部分代码，可以看到此处收到了第一部分代码发出的 transform ，然后把其中的值转换为第二个乌龟的 twist 速度信息发出去了，具体的小乌龟Spawn 服务应该是召唤第二个小乌龟，然后接收twist 信息进行运动的功能，这个就不仔细描写了。

tf2 的官方资料链接

最终，ros中的tf功能就是把不同父类和子类之间的坐标转换关系发出来，然后接收端根据发出的转换信息，对数据进行处理。比如 A为B坐标系父类，B为C父类，同时D也是A子类，只要知道C到B，B到A，以及D到A的转换，就可以计算出C到D的转换。但是这样，tf有一个缺点，如果一个消息由很多个需要转换的点，tf是无法同时转换这么多点的，可以自己计算出对应的转换矩阵，然后用eigen单独对每个需要转换的坐标计算。