ROS中的坐标转换1

​

1、坐标转换

坐标转换是指坐标系之间的平移以及旋转关系，如坐标系A,B,C。A，B之间存在一个转换关系 $T_B^A$,B与C之间存在转换关系$T_C^B$,我们知道了B相对于A的关系，C相对于B的转换关系，同样我们可以A，C之间也存在着转换关系$T_C^A$. 根据两者之间的转换关系可以得到$T_C^A$=$T_B^A$ * $T_C^B$.

如图所示

2、lookupTransform获取坐标系之间的转换关系

2.1 lookupTransform

原型：

void Transformer:: lookuptransform (const std::string & target_frame,const std::string & source_frame, const ros::Time & Time, StampedTransform & transform)
1

• target_frame：目标坐标系，数据应该转到的坐标系
• source_frame：源坐标系，数据来源坐标系
• Time：时间，使用 ros::Time(0)，使用ros::time::now()存在问题
• transform：存在转换关系

使用步骤：

1. 获取TransformListener监听器

tf::TransformListener*  tfListener= new tf::TransformListener;
1

2. 定义StampedTransform保存关系

 tf::StampedTransform stfBaseToLink2, stfBaseToLink1, stfLink1ToLink2;
1

3. 使用

    while (tferr) {
        tferr=false;
        try {
              tfListener->lookupTransform("base_link", "link1", ros::Time(0), tfLink2WrtBaseLink);
            } catch(tf::TransformException &exception) {
                ROS_WARN("%s; retrying...", exception.what());
                tferr=true;
                ros::Duration(0.5).sleep(); 
                continue;            
            }   
    }
1
2
3
4
5
6
7
8
9
10
11

注意：要使用try…catch进行获取，而且一次可能获取不到。

WGS84转北京54的Alberts投影坐标

application/x-zip 4星 超过75%的资源 545KB

下载

3、基于lookupTransform证明 $T_C^A$=$T_B^A$ * $T_C^B$.

1. 获取B相对于A的转换关系（$T_B^A$ ）

    while (tferr) {
        tferr=false;
        try {
              tfListener->lookupTransform("base_link", "link1", ros::Time(0), tfLink2WrtBaseLink);
            } catch(tf::TransformException &exception) {
                ROS_WARN("%s; retrying...", exception.what());
                tferr=true;
                ros::Duration(0.5).sleep();
                continue;            
            }   
    }
1
2
3
4
5
6
7
8
9
10
11

2. 获取C相对于B转换关系（$T_C^B$）

    while (tferr) {
        tferr=false;
        try {
                tfListener->lookupTransform("link1", "link2", ros::Time(0), stfLink1ToLink2);
            } catch(tf::TransformException &exception) {
                ROS_WARN("%s; retrying...", exception.what());
                tferr=true;
                ros::Duration(0.5).sleep(); // sleep for half a second
                continue;            
            }   
    }
1
2
3
4
5
6
7
8
9
10
11

3. 进行把StampedTransform转换Transform

tf::Transform tfBaseToLink1(tfLink2WrtBaseLink.getBasis(),tfLink2WrtBaseLink.getOrigin())
tf::Transform tfBaseToLink2(stfLink1ToLink2.getBasis(),stfLink1ToLink2.getOrigin())
1
2

4. 进行$T_B^A$ * $T_C^B$

altTfBaseToLink2 = tfBaseToLink1*tfLink1ToLink2;
1

5. 获取C相对于A的转换关系（$T_C^A$）

    while (tferr) {
        tferr=false;
        try {
                tfListener->lookupTransform("base_link", "link2", ros::Time(0), stfBaseToLink2);
            } catch(tf::TransformException &exception) {
                ROS_WARN("%s; retrying...", exception.what());
                tferr=true;
                ros::Duration(0.5).sleep(); // sleep for half a second
                continue;            
            }   
    }
1
2
3
4
5
6
7
8
9
10
11

6. 进行把StampedTransform转换Transform

tf::Transform tfBaseToLink2(stfBaseToLink2.getBasis(),stfBaseToLink2.getOrigin())
1

7. 打印结果
   
   全部代码（修改于ROS机器人编程示例）

#include 
#include 
#include 
#include 

#include  //ALWAYS need to include this

#include 
#include 
#include 
#include 
#include 
#include
using namespace std;

void printTf(tf::Transform tf) {
    tf::Vector3 tfVec;
    tf::Matrix3x3 tfR;
    tf::Quaternion quat;
    tfVec = tf.getOrigin();
    cout<<"vector from reference frame to to child frame: "<<tfVec.getX()<<","<<tfVec.getY()<<","<<tfVec.getZ()<<endl;
    tfR = tf.getBasis();
    cout<<"orientation of child frame w/rt reference frame: "<<endl;
    tfVec = tfR.getRow(0);
    cout<<tfVec.getX()<<","<<tfVec.getY()<<","<<tfVec.getZ()<<endl;
    tfVec = tfR.getRow(1);
    cout<<tfVec.getX()<<","<<tfVec.getY()<<","<<tfVec.getZ()<<endl;    
    tfVec = tfR.getRow(2);
    cout<<tfVec.getX()<<","<<tfVec.getY()<<","<<tfVec.getZ()<<endl; 
    quat = tf.getRotation();
    cout<<"quaternion: " <<quat.x()<<", "<<quat.y()<<", "
            <<quat.z()<<", "<<quat.w()<<endl;   
}
tf::Transform get_tf_from_stamped_tf(tf::StampedTransform sTf) {
   tf::Transform tf(sTf.getBasis(),sTf.getOrigin()); //construct a transform using elements of sTf
   return tf;
}
void printStampedTf(tf::StampedTransform sTf){
    tf::Transform tf;
    cout<<"frame_id: "<<sTf.frame_id_<<endl;
    cout<<"child_frame_id: "<<sTf.child_frame_id_<<endl; 
    tf = get_tf_from_stamped_tf(sTf); //extract the tf from the stamped tf  
    printTf(tf); //and print its components      
    }

//fnc to print components of a stamped pose
void printStampedPose(geometry_msgs::PoseStamped stPose){
    cout<<"frame id = "<<stPose.header.frame_id<<endl;
    cout<<"origin: "<<stPose.pose.position.x<<", "<<stPose.pose.position.y<<", "<<stPose.pose.position.z<<endl;
    cout<<"quaternion: "<<stPose.pose.orientation.x<<", "<<stPose.pose.orientation.y<<", "
            <<stPose.pose.orientation.z<<", "<<stPose.pose.orientation.w<<endl;
}
int main(int argc, char * argv[])
{
    ros::init(argc, argv, "listentext");
    ros::NodeHandle nh;
    tf::TransformListener*  tfListener= new tf::TransformListener;
    tf::StampedTransform stfBaseToLink2, stfBaseToLink1, stfLink1ToLink2;
    tf::StampedTransform testStfBaseToLink2;

    tf::Transform tfBaseToLink1, tfLink1ToLink2, tfBaseToLink2, altTfBaseToLink2;
    bool tferr=true;
    ROS_INFO("waiting for tf between link2 and base_link...");
    tf::StampedTransform tfLink2WrtBaseLink; 
    while (tferr) {
        tferr=false;
        try {
                //try to lookup transform, link2-frame w/rt base_link frame; this will test if
            // a valid transform chain has been published from base_frame to link2
                tfListener->lookupTransform("base_link", "link1", ros::Time(0), tfLink2WrtBaseLink);
            } catch(tf::TransformException &exception) {
                ROS_WARN("%s; retrying...", exception.what());
                tferr=true;
                ros::Duration(0.5).sleep(); // sleep for half a second
                continue;            
            }   
    }
    // printStampedTf(stfBaseToLink1);
    tfBaseToLink1 = get_tf_from_stamped_tf(tfLink2WrtBaseLink);
    tferr = true;
    while (tferr) {
        tferr=false;
        try {
                tfListener->lookupTransform("link1", "link2", ros::Time(0), stfLink1ToLink2);
            } catch(tf::TransformException &exception) {
                ROS_WARN("%s; retrying...", exception.what());
                tferr=true;
                ros::Duration(0.5).sleep(); // sleep for half a second
                continue;            
            }   
    }
    tfLink1ToLink2 = get_tf_from_stamped_tf(stfLink1ToLink2);
    tferr = true;
    while (tferr) {
        tferr=false;
        try {
                tfListener->lookupTransform("base_link", "link2", ros::Time(0), stfBaseToLink2);
            } catch(tf::TransformException &exception) {
                ROS_WARN("%s; retrying...", exception.what());
                tferr=true;
                ros::Duration(0.5).sleep(); // sleep for half a second
                continue;            
            }   
    }
    printStampedTf(stfBaseToLink2);
    tfBaseToLink2 = get_tf_from_stamped_tf(stfBaseToLink2);
    cout << endl << "baseLink to Link2:" << endl;
    printTf(tfBaseToLink2);
    cout<<"==============================";
    altTfBaseToLink2 = tfBaseToLink1*tfLink1ToLink2;
    cout << endl << "baseLinkToLin1 * Link1ToLin2: " << endl;
    printTf(altTfBaseToLink2);

    return 0;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116

​