坐标转换是指坐标系之间的平移以及旋转关系，如坐标系A,B,C。A，B之间存在一个转换关系 T B A T_{B}^{A} TBA,B与C之间存在转换关系 T C B T_{C}^{B} TCB,我们知道了B相对于A的关系，C相对于B的转换关系，同样我们可以A，C之间也存在着转换关系 T C A T_{C}^{A} TCA. 根据两者之间的转换关系可以得到 T C A T_{C}^{A} TCA= T B A T_{B}^{A} TBA * T C B T_{C}^{B} TCB.
如图所示
原型：
void Transformer:: lookuptransform (const std::string & target_frame,const std::string & source_frame, const ros::Time & Time, StampedTransform & transform)
使用步骤：
tf::TransformListener* tfListener= new tf::TransformListener;
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进行获取，而且一次可能获取不到。
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;
}
}
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;
}
}
tf::Transform tfBaseToLink1(tfLink2WrtBaseLink.getBasis(),tfLink2WrtBaseLink.getOrigin())
tf::Transform tfBaseToLink2(stfLink1ToLink2.getBasis(),stfLink1ToLink2.getOrigin())
altTfBaseToLink2 = tfBaseToLink1*tfLink1ToLink2;
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;
}
}
tf::Transform tfBaseToLink2(stfBaseToLink2.getBasis(),stfBaseToLink2.getOrigin())
#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;
}