URDF和TF

TF测试

我在学习ROS时候发现，URDF和TF都可以发布TF，我是指执行如下命令时候有Tree。然后我就很疑惑，既然URDF可以表示Frame的坐标转换关系，还需要单独TF干吗？

$ rosrun rqt_tf_tree rqt_tf_tree

然后我看了下面这个文章，说实话，没有解决我的问题，但是小例子，值得学习。
链接

1-向量、四元数、旋转矩阵

coordinate_transform_vector.cpp

#include <ros/ros.h>
#include <tf/tf.h>
#include <iostream>
using namespace std;
int main(int argc, char** argv){
    //初始化
    ros::init(argc, argv, "coordinate_transformation");
    ros::NodeHandle node;
    tf::Vector3 v1(1,1,1);
    tf::Vector3 v2(1,0,1);

    //第1部分，定义空间点和空间向量
    cout << "第1部分，定义空间点和空间向量" << endl;
    //1.1计算两个向量的点积
    cout << "向量v1:" << "("<< v1[0] << ","<< v1[1]<<","<< v1[2] << ")"<<endl;
    cout << "向量v2:" << "("<< v2[0] << ","<< v2[1]<<","<< v2[2] << ")"<<endl;
    cout << "两个向量的点积" << tf::tfDot(v1,v2) << endl;
    //1.2计算向量的模
    cout << "向量v2的模值" << v2.length() << endl;
    //1.3求与已知向量同方向的单位向量
    tf::Vector3 v3;
    v3 = v2.normalize();
    cout << "与向量v2的同方向的单位向量v3：" << "("<< v3[0] << ","<< v3[1]<<","<< v3[2] << ")"<<endl;
    //1.4计算两个向量的夹角
    cout << "两个向量的夹角(弧度)：" << tf::tfAngle(v1, v2) << endl;
    //1.5计算两个向量的距离
    cout << "两个向量的距离：" << tf::tfDistance2(v1,v2) << endl;
    //1.6计算两个向量的乘积
    tf::Vector3 v4;
    v4 = tf::tfCross(v1, v2);
    cout << "两个向量的乘积：" << "("<< v4[0] << ","<< v4[1]<<","<< v4[2] << ")"<<endl;

    cout << "---------------------------------------------" << endl;

    //第2部分，定义四元数
    cout << "第2部分，定义四元数" << endl;
    //2.1由欧拉角计算四元数
    tfScalar yaw, pitch, roll;
    yaw = 0;
    pitch = 0;
    roll = 0;
    cout << "欧拉角rpy(" << roll << "," << pitch << "," << yaw << ")";
    tf::Quaternion q;
    q.setRPY(yaw, pitch, roll);
    cout << "，转化到四元数q:" << "(" << q[3] << ","<<q[0] << "," << q[1]<<"," << q[2]<<")" << endl;
    //2.2由四元数获得旋转轴
    tf::Vector3 v5;
    v5 = q.getAxis();
    cout << "四元数旋转轴v5" << "("<< v5[0] << ","<< v5[1]<<","<< v5[2] << ")"<<endl;
    //2.3由旋转轴和旋转角来估计四元数
    tf::Quaternion q2;
    q2.setRotation(v5, 1.570796);
    cout << "四元数旋转轴v5和旋转角度90度，转化到四元数q2：" << "("<<q2[3]<<","<< q2[0] << ","<< q2[1]<<","<< q2[2] << ")"<<endl;
    //2.4四元数转欧拉角
    double yaw1, pitch1, roll1;
    tf::Matrix3x3(q).getRPY(roll1,pitch1,yaw1);
    cout << "四元数转欧拉角：(" << roll1 << "," << pitch1 << "," << yaw1<<")" <<endl;

    cout << "---------------------------------------------" << endl;

    //第3部分，定义旋转矩阵
    //3.1由旋转轴和旋转角来估计四元数
    tf::Quaternion q3(1,0,0,0);
    tf::Matrix3x3 Matrix;
    tf::Vector3 v6, v7, v8;
    Matrix.setRotation(q3);
    v6 = Matrix[0];
    v7 = Matrix[1];
    v8 = Matrix[2];
    cout << "四元数q2对应的旋转矩阵M：" << endl;
    cout << v6[0] << ","<< v6[1] << ","<< v6[1] << endl;
    cout << v7[0] << ","<< v7[1] << ","<< v7[1] << endl;
    cout << v8[0] << ","<< v8[1] << ","<< v8[1] << endl;
    //3.2通过旋转矩阵求欧拉角
    tfScalar m_yaw, m_pitch, m_roll;
    Matrix.getEulerYPR(m_yaw, m_pitch, m_roll);
    cout << "由旋转矩阵M，得到欧拉角RPY(" << m_roll << "," << m_yaw << "," << m_pitch << ")" << endl;
    return 0;
}

2-欧拉角转四元数

Euler2Quaternion.cpp

// #include <ros/ros.h>
// #include <tf/tf.h>
// #include <iostream>
// using namespace std;
// int main(int argc, char ** argv){
//     ros::init(argc, argv, "Euler2Quaternion");
//     ros::NodeHandle node;
//     geometry_msgs::Quaternion q;
//     double roll,pitch, yaw;
//     while(ros::ok()){
//         //输入一个相对原点的位置
//         cout << "输入的欧拉角:roll、pitch、yaw:" ;
//         cin >> roll >> pitch >> yaw;
//         //输入欧拉角，转化为四元数在终端输出
//         q = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
//         //ROS_INFO("输出的四元数为：w=%d,y=%d,z=%d", q.w,q.x,q.y,q.z);
//         cout << "输出的四元数为：w=" << q.w << ",x=" << q.x << ",y=" << q.y << ",z=" << q.z << endl;
//         ros::spinOnce();
//     }
//     return 0;
// }

#include <ros/ros.h>
#include <tf/tf.h>
#include <iostream>
using namespace std;
int main(int argc, char ** argv){
    ros::init(argc, argv, "Euler2Quaternion");
    ros::NodeHandle node;
    geometry_msgs::Quaternion q;
    // double roll,pitch, yaw;
    tf::Vector3 t;
    while(ros::ok()){
        //输入一个相对原点的位置
        cout << "输入的欧拉角:roll、pitch、yaw:" ;
        cin >> t[0] >> t[1] >> t[2];
        //输入欧拉角，转化为四元数在终端输出
        q = tf::createQuaternionMsgFromRollPitchYaw(t[0], t[1], t[2]);
        //ROS_INFO("输出的四元数为：w=%d,y=%d,z=%d", q.w,q.x,q.y,q.z);
        cout << "输出的四元数为：w=" << q.w << ",x=" << q.x << ",y=" << q.y << ",z=" << q.z << endl;
        ros::spinOnce();
    }
    return 0;
}

3-四元数转欧拉角

Quaternion2Euler.cpp

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <tf/tf.h>
#include <iostream>
using namespace std;
int main(int argc, char ** argv){
    //初始化
    ros::init(argc, argv, "Quaternion2Euler");
    ros::NodeHandle node;
    nav_msgs::Odometry position;
    tf::Quaternion RQ2;
    double roll, pitch, yaw;
    while(ros::ok()){
        //输入一个四元数原点
        cout << "输入一个四元数:w,x,y,z:" <<endl;
        cin >> position.pose.pose.orientation.w
            >> position.pose.pose.orientation.x
            >> position.pose.pose.orientation.y
            >> position.pose.pose.orientation.z;
        //输入四元数，转化为欧拉角，在终端输出
        tf::quaternionMsgToTF(position.pose.pose.orientation, RQ2);
        // tf::Vector3 m_vector3;
        // m_vector3 = RQ2.getAxis();
        tf::Matrix3x3(RQ2).getRPY(roll,pitch,yaw);
        cout << "输出的欧拉角为：" << "roll = " << roll <<", pitch = " << pitch <<", yaw = " << yaw << endl;
        ros::spinOnce();
    }
    return 0;
}

4-TF发布

tf_broadcaster.cpp

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>
#include <iostream>
using namespace std;

int main(int argc, char** argv){
    //初始化
    ros::init(argc, argv, "tf_broadcaster");
    ros::NodeHandle node;
    static tf::TransformBroadcaster br;
    tf::Transform transform;
    tf::Quaternion q;
    //定义初始坐标和角度
    double roll = 0, pitch =0, yaw = 0, x= 1.0, y = 2.0, z = 3.0;
    ros::Rate rate(1);
    while(ros::ok()){
        yaw+=0.1;//每经过1s开始一次变换
        //输入欧拉角，转化成四元数在终端输出
        q.setRPY(roll, pitch, yaw);
        transform.setOrigin(tf::Vector3(x,y,z));
        transform.setRotation(q);
        cout << "发布tf变化：sendTransform函数" << endl;
        br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base_link","link1"));
        cout << "坐标原点：x=" << x << ", y=" << y << ", z=" << z << endl;
        cout << "输出的欧拉角为：roll=" << roll << ", pitch=" << pitch << ", yaw=" << yaw << endl;
        cout << "输出的四元数为：w=" << q[3] << ", x=" << q[0] << ", y=" << q[1] << ", z=" << q[2] << endl;
        rate.sleep();
        ros::spinOnce(); 
    }
    return 0;
}

5-TF监听

tf_listener.cpp

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <iostream>
using namespace std;

int main(int argc, char** argv){
    ros::init(argc, argv, "tf_listener");
    ros::NodeHandle node;
    tf::TransformListener listener;
    //1.阻塞直到frame相通
    cout << "1.阻塞直到frame相通" << endl;
    listener.waitForTransform("base_link","link1",ros::Time(0),ros::Duration(4.0));
    ros::Rate rate(1);
    while(node.ok()){
        tf::StampedTransform transform;
        try{
            //2.监听对应的tf，返回平移和旋转
            cout << "2.监听对应的tf，返回平移和旋转" << endl;
            listener.lookupTransform("/base_link","/link1",ros::Time(0),transform);
        }
        catch(tf::TransformException &ex){
            ROS_ERROR("%s", ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }
        cout << "输出的位置坐标：x=" << transform.getOrigin().x() << ",y=" << transform.getOrigin().y() << ",z=" << transform.getOrigin().z() << endl;
        cout << "输出的旋转的四元数：w=" << transform.getRotation().getW() 
                            << ",x=" << transform.getRotation().getX()
                            << ",y=" << transform.getRotation().getY() 
                            << ",z=" << transform.getRotation().getZ() << endl;
        rate.sleep();
    }
    return 0;
}

CMakeLists

cmake_minimum_required(VERSION 2.8.3)
project(tf_learning)

add_compile_options(-std=c++11)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  tf
)

###################################
## catkin specific configuration ##
###################################
catkin_package(
    INCLUDE_DIRS include
    CATKIN_DEPENDS roscpp tf std_msgs geometry_msgs message_runtime nav_msgs
)

###########
## Build ##
###########
include_directories(
  include
  ${catkin_INCLUDE_DIRS}
)

add_executable(coordinate_transformation_vector src/coordinate_transformation_vector.cpp)
target_link_libraries(coordinate_transformation_vector  ${catkin_LIBRARIES})

add_executable(Euler2Quaternion src/Euler2Quaternion.cpp)
target_link_libraries(Euler2Quaternion  ${catkin_LIBRARIES})


add_executable(Quaternion2Euler src/Quaternion2Euler.cpp)
target_link_libraries(Quaternion2Euler  ${catkin_LIBRARIES})

add_executable(tf_broadcaster src/tf_broadcaster.cpp)
target_link_libraries(tf_broadcaster  ${catkin_LIBRARIES})

add_executable(tf_listener src/tf_listener.cpp)
target_link_libraries(tf_listener  ${catkin_LIBRARIES})

package.xml

<?xml version="1.0"?>
<package format="2">
  <name>tf_learning</name>
  <version>0.0.0</version>
  <description>The tf_learning package</description>

  <maintainer email="xxx@xxx.com">fjc</maintainer>

  <license>MIT</license>

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>tf</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>geometry_msgs</build_depend>
  <build_depend>nav_msgs</build_depend>
  <build_depend>message_generation</build_depend>

  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>tf</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
  <build_export_depend>geometry_msgs</build_export_depend>
  <build_export_depend>nav_msgs</build_export_depend>

  <exec_depend>message_runtime</exec_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>tf</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>geometry_msgs</exec_depend>
  <exec_depend>nav_msgs</exec_depend>

</package>