ROS中的坐标转换1

最新推荐文章于 2024-04-20 19:59:02 发布

晓理紫

最新推荐文章于 2024-04-20 19:59:02 发布

阅读量4.3k

点赞数 4

分类专栏：机器人ROS 文章标签：自动驾驶人工智能机器学习

本文链接：https://blog.csdn.net/u011573853/article/details/126333900

版权

机器人ROS 专栏收录该内容

39 篇文章 6 订阅

订阅专栏

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)

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

使用步骤：

获取TransformListener监听器

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

定义StampedTransform保存关系

 tf::StampedTransform stfBaseToLink2, stfBaseToLink1, stfLink1ToLink2;

使用

    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;            
            }   
    }

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

3、基于lookupTransform证明 $T_{C}^{A}$ = $T_{B}^{A}$ * $T_{C}^{B}$ .

获取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;            
            }   
    }

获取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;            
            }   
    }

进行把StampedTransform转换Transform

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

进行 $T_{B}^{A}$ * $T_{C}^{B}$

altTfBaseToLink2 = tfBaseToLink1*tfLink1ToLink2;

获取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;            
            }   
    }

进行把StampedTransform转换Transform

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

打印结果

全部代码（修改于ROS机器人编程示例）

#include <math.h>
#include <stdlib.h>
#include <string>
#include <vector>

#include <ros/ros.h> //ALWAYS need to include this

#include <geometry_msgs/Point.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/transform_listener.h>
#include <tf/LinearMath/Vector3.h>
#include<iostream>
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;
}