ROS-坐标转换


坐标转换

    坐标转换是机器人学中的基本概念,因为机器人中存在大量的组件,每个组件有不同的坐标系。一个坐标系可以通过平移和旋转得到另一个坐标系,平移和旋转可以通过变换矩阵实现。有关这部分的知识可参阅:三维空间中刚体运动


ROS中坐标变换

    下面是PR2机器人的坐标系一览:

tf_wiki.png

    其本质上可以抽象为一个坐标系树(tf tree):

tftree.png

    ROS中机器人模型包含大量的部件,这些部件统称之为link,每一个link上面对应着一个frame, 即一个坐标系。l可以看到又很多的frame,错综复杂的铺置在机器人的各个link上,维护各个坐标系之间的关系,就要靠着tf tree来处理,维护着各个坐标系之间的联通。

    每一个圆圈代表一个frame,对应着机器人上的一个link,任意的两个frame之间都必须是联通的。每两个frame之间都有一个broadcaster,为了使得两个frame之间能够正确连通,中间都会有一个Node来发布坐标转换信息。如果缺少Node来发布消息维护连通,那么这两个frame之间的连接就会断掉。broadcaster就是一个publisher,如果两个frame之间发生了相对运动,broadcaster就会发布相关消息。

    坐标系中间的broadcaster发布是消息是TransformStampde.msg,内容如下:

std_mags/Header header
        uint32 seq
        time stamp
        string frame_id
string child_frame_id
geometry_msgs/Transform transform
        geometry_msgs/Vector3 translation
                float64 x
                float64 y
                float64 z
        geometry_msgs/Quaternion rotation
                float64 x
                float64 y
                flaot64 z
                float64 w

    上面的消息中:header定义了序号,时间,frame的名称,child_frame的名称。这两个frame之间的变换由geometry_msgs/Transform来定义,Vector3三维向量表示平移,Quaternion四元数表示旋转。

    一个机器人系统里有很多不同frame之间的broadcaster在发布坐标变换的tf,他们拼接得到tf tree。TF tree的数据类型是tf/tfMessage.msg(老版)和tf2_msgs/TFMessage.msg(新版)。标准格式如下:

首先header定义了序号,时间以及frame的名称.接着还写了child_frame,这两个frame之间要做那种变换就是由geometry_msgs/Transform来定义.Vector3三维向量表示平移,Quaternion四元数表示旋转

geometry_msgs/TransformStamped[] transforms
        std_msgs/Header header
                uint32 seq
                time stamp
                string frame_id
        string child_frame_id
        geometry_msgs/Transform transform
                geometry_msgs/Vector3 translation
                        float64 x
                        float64 y
                        float64 z
                geometry_msgs/Quaternion rotation
                        float64 x
                        float64 y
                        flaot64 z
                        float64 w

    如上,tf tree就是TransformStamped数组。


ROS TF常见功能

    TF功能包中提供了一些常用功能用来调试和创建TF变换。

1.tf_monitor

    该节点可以查看坐标系广播的发布状态。

rosrun tf tf_monitor #查看所有广播器的发布
rosrun tf tf_monitor source_frame target_frame #查看两个坐标系的广播

2.tf_echo

    该节点可以查看指定坐标系之间的变换关系,如:

rosrun tf tf_echo turtle3 turtle1

    结果:

At time 1576577139.888
- Translation: [0.000, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.952, 0.306]
            in RPY (radian) [-0.000, -0.000, 2.519]
            in RPY (degree) [-0.000, -0.000, 144.307]

3.static_transfrm_publisher

    发布两个坐标系之间的静态坐标变换。

4.view_frames

    生成pdf显示TF树。如:

rosrun tf view_frames

     结果:

2019-12-17 17-51-03屏幕截图.png


Turtlesim功能包

    turtlesim是ROS自带的一个用于新手教学的功能包,核心就是用键盘控制小乌龟运动。该功能包有两个节点,详细的信息可以参阅官方文档:http://wiki.ros.org/turtlesim 。



TF的turtlesim跟随实例

    这里实现的是在turtlesim里面创建多个乌龟,每个乌龟的parent_frame都是世界坐标系,在TF tree上查找某两个乌龟之间的坐标变换,从而实现乌龟的跟随。

1.创建功能包

    在工作空间创建功能包,依赖:roscpp tf turtlesim。这里创建的功能包为tf_test


2.节点:生成乌龟。

    启动turtlesim turtlesim_node之后,只默认创建一个乌龟,名为/turtle1。这里通过调用turtlesim::Spawn服务,在仿真环境中可以创建新的乌龟。节点tf_test/src/turtle_create.cpp:

#include <ros/ros.h>
#include <turtlesim/Spawn.h>
int main(int argc, char** argv)
{
    ros::init(argc, argv, "node_create_turtle");
    ros::NodeHandle node;
    // 通过服务调用,产生第二只乌龟turtle2
    ros::service::waitForService("spawn"); //等待spawn服务可用
    ros::ServiceClient add_turtle =node.serviceClient<turtlesim::Spawn>("spawn");
    turtlesim::Spawn srv;//默认参数
    add_turtle.call(srv);
    return 0;
};


3.节点:发布坐标转换

    前面说过,每个frame之间必须要有一个广播节点发布坐标转换,这里的乌龟的坐标系在tf tree上面是世界坐标系的孩子。广播器如下(tf_test/src/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)
{
    // tf广播器
    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];
    // 订阅乌龟的pose信息
    ros::NodeHandle node;
    ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
    ros::spin();
    return 0;
};

    从官方文档可知,每个乌龟会发布消息turtle_name/pose,其内容turtlesim/Pose如下:

//乌龟在世界坐标系中的坐标
float32 x
float32 y
//乌龟在世界坐标系中,绕z轴旋转的角度
float32 theta
//线速度和角速度
float32 linear_velocity
float32 angular_velocity

    回调函数里面,首先定义一个广播器br,接着定义一个坐标变换transform。setOrigin函数设置平移变换,设坐标变换A->B,那么该函数就是设置B坐标系原点在A坐标系的坐标。接着定义一个四元数q。setRPY即把B坐标系的相对旋转角度设置为转换四元数。最后发布坐标变换。


4.节点:监听坐标变换,并将坐标转换用于控制乌龟运动。

    通过当监听到广播器发出的坐标转换时,查找两个乌龟间的坐标变换关系,并将该坐标变换用于控制乌龟的运动,使一只乌龟能跟上另一只乌龟。因此这里还定义一个topic,叫做turtle_name/cmd_vel用于控制乌龟的运动。代码tf_test/src/turtle_tf_listener.cpp:

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
int main(int argc, char** argv){
    ros::init(argc, argv, "my_tf_listener");
    ros::NodeHandle node;
    if (argc != 3){
        ROS_ERROR("need turtle name as argument"); 
        return -1;
    };
    //A跟随B,获取乌龟A和乌龟B的名字
    std::string turtleA = argv[1];
    std::string turtleB = argv[2];
    //定义turtle的速度控制发布器
    ros::Publisher turtle_vel =node.advertise<geometry_msgs::Twist>(turtleA+"/cmd_vel", 10);
    //tf监听器
    tf::TransformListener listener;
    ros::Rate rate(10.0);
    while (node.ok()){
        tf::StampedTransform transform;
        try{
            // 查找turtleA与turtleB的坐标变换
            listener.waitForTransform("/"+turtleA, "/"+turtleB, ros::Time(0), ros::Duration(3.0));
            listener.lookupTransform("/"+turtleA, "/"+turtleB, ros::Time(0), transform);
        }
        catch (tf::TransformException &ex){
            ROS_ERROR("%s",ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }
        // 根据turtle1和turtle2之间的坐标变换,计算turtle2需要运动的线速度和角速度
        //并发布速度控制指令,使turtle2向turtle1移动
        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;
};

    代码的解释可以看注释。


5.启动文件。

    tf_test/launch/turtlesim_tf_test.launch

 <launch>
    <!-- 乌龟仿真器 -->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
    <!-- 键盘控制 -->
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
    
    <!--再另外创建两只乌龟-->
    <node pkg="tf_test" type="turtle_create" name="turtle_create2" />
    <node pkg="tf_test" type="turtle_create" name="turtle_create3" />
    
    <!-- 三只乌龟的tf广播 -->
    <node pkg="tf_test" type="turtle_tf_broadcaster" args="/turtle1" name="turtle1_tf_broadcaster" />
    <node pkg="tf_test" type="turtle_tf_broadcaster" args="/turtle2" name="turtle2_tf_broadcaster" />
    <node pkg="tf_test" type="turtle_tf_broadcaster" args="/turtle3" name="turtle3_tf_broadcaster" />
    
    <!-- 监听tf广播,并且控制turtle移动 -->
    <node pkg="tf_test" type="turtle_tf_listener" args="turtle2 turtle1" name="listener2" />
    <node pkg="tf_test" type="turtle_tf_listener" args="turtle3 turtle2" name="listener3" />
 
 </launch>


6.编译并运行。

    在tf_test/CMakeLists.txt里面设置编译选项:

add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})
add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})
add_executable(turtle_create src/turtle_create.cpp)
target_link_libraries(turtle_create ${catkin_LIBRARIES})

    然后编译工作空间,编译完后直接运行launch文件即可查看效果。

    结果如下:

2019-12-17 17-36-09屏幕截图1.png

    可以看到键盘控制最左边乌龟的运动,中间的乌龟跟随左边的,右边的乌龟跟随中间的。





参考文献

[0]中科院软件所,重德智能公司.中国大学MOOC---《机器人操作系统入门》课程讲义.https://sychaichangkun.gitbooks.io/ros-tutorial-icourse163/content/

[1]胡春旭.ROS机器人开发实践.机械工业出版社

首页 所有文章 机器人 计算机视觉 自然语言处理 机器学习 编程随笔 关于