ROS学习笔记八：The Tranform Listener
ROS学习笔记八：The Tranform Listener
用于手眼坐标之间的转换：
- 获取对象相对于相机的位姿信息
- 获取夹持器相对于基底的位姿信息
- 计算目标与夹持器之间的差异（空间上的）
- 基于上诉差异（空间上的）创建一个轨迹来定位对象的位置
以下通过一个例子说明是如何在三个杆件间转换坐标的：
三个连杆分别为：base_link 、 link1 、 link2
① 先创建一个名叫example_tf_listener的包
cd ros_ws
cs_create_pkg example_tf_listener roscpp std_msgs
② 创建一个头文件定义一个类（把获取对象位姿信息和坐标转换的函数封装起来）
cd example_tf_listener/src
touch example_tf_listener.h
#ifndef EXAMPLE_TF_LISTENER_H_
#define EXAMPLE_TF_LISTENER_H_
//用到的一些头文件
#include //必加
#include ③ 创建一个执行类中方法的cpp文件
touch example_tf_listener_fncs.cpp
//在这个cpp文件中执行类中的成员函数
#include "example_tf_listener.h"
using namespace std;
//constructor: don't need nodehandle here, but could be handy if want to add a subscriber
DemoTfListener::DemoTfListener(ros::NodeHandle* nodehandle):nh_(*nodehandle)
{
ROS_INFO("in class constructor of DemoTfListener");
tfListener_ = new tf::TransformListener; //create a transform listener and assign its pointer
//here, the tfListener_ is a pointer to this object, so must use -> instead of "." operator
//somewhat more complex than creating a tf_listener in "main()", but illustrates how
// to instantiate a tf_listener within a class
// wait to start receiving valid tf transforms between base_link and link2:
// this example is specific to our mobot, which has a base_link and a link2
// lookupTransform will through errors until a valid chain has been found from target to source frames
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", "link2", 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
ros::spinOnce();
}
}
ROS_INFO("tf is good");
// from now on, tfListener will keep track of transforms; do NOT need ros::spin(), since
// tf_listener gets spawned as a separate thread
}
//some conversion utilities:
//getting a transform from a stamped transform is trickier than expected--there is not "get" fnc for transform
tf::Transform DemoTfListener::get_tf_from_stamped_tf(tf::StampedTransform sTf) {
tf::Transform tf(sTf.getBasis(),sTf.getOrigin()); //construct a transform using elements of sTf
return tf;
}
//a transform describes the position and orientation of a frame w/rt a reference frame
//a Pose is a position and orientation of a frame of interest w/rt a reference frame
// can re-interpret a transform as a named pose using this example function
// the PoseStamped also has a header, which includes naming the reference frame
geometry_msgs::PoseStamped DemoTfListener::get_pose_from_transform(tf::StampedTransform tf) {
//clumsy conversions--points, vectors and quaternions are different data types in tf vs geometry_msgs
geometry_msgs::PoseStamped stPose;
geometry_msgs::Quaternion quat; //geometry_msgs object for quaternion
tf::Quaternion tfQuat; // tf library object for quaternion
tfQuat = tf.getRotation(); // member fnc to extract the quaternion from a transform
quat.x = tfQuat.x(); // copy the data from tf-style quaternion to geometry_msgs-style quaternion
quat.y = tfQuat.y();
quat.z = tfQuat.z();
quat.w = tfQuat.w();
stPose.pose.orientation = quat; //set the orientation of our PoseStamped object from result
// now do the same for the origin--equivalently, vector from parent to child frame
tf::Vector3 tfVec; //tf-library type
geometry_msgs::Point pt; //equivalent geometry_msgs type
tfVec = tf.getOrigin(); // extract the vector from parent to child from transform
pt.x = tfVec.getX(); //copy the components into geometry_msgs type
pt.y = tfVec.getY();
pt.z = tfVec.getZ();
stPose.pose.position= pt; //and use this compatible type to set the position of the PoseStamped
stPose.header.frame_id = tf.frame_id_; //the pose is expressed w/rt this reference frame
stPose.header.stamp = tf.stamp_; // preserve the time stamp of the original transform
return stPose;
}
//a function to multiply two stamped transforms; this function checks to make sure the
// multiplication is logical, e.g.: T_B/A * T_C/B = T_C/A
// returns false if the two frames are inconsistent as sequential transforms
// returns true if consistent A_stf and B_stf transforms, and returns result of multiply in C_stf
// The reference frame and child frame are populated in C_stf accordingly
bool DemoTfListener::multiply_stamped_tfs(tf::StampedTransform A_stf,
tf::StampedTransform B_stf, tf::StampedTransform &C_stf) {
tf::Transform A,B,C; //simple transforms--not stamped
std::string str1 (A_stf.child_frame_id_); //want to compare strings to check consistency
std::string str2 (B_stf.frame_id_);
if (str1.compare(str2) != 0) { //SHOULD get that child frame of A is parent frame of B
std::cout<<"can't multiply transforms; mismatched frames"<<endl;
std::cout << str1 << " is not " << str2 << '\n';
return false;
}
//if here, the named frames are logically consistent
A = get_tf_from_stamped_tf(A_stf); // get the transform from the stamped transform
B = get_tf_from_stamped_tf(B_stf);
C = A*B; //multiplication is defined for transforms
C_stf.frame_id_ = A_stf.frame_id_; //assign appropriate parent and child frames to result
C_stf.child_frame_id_ = B_stf.child_frame_id_;
C_stf.setOrigin(C.getOrigin()); //populate the origin and orientation of the result
C_stf.setBasis(C.getBasis());
C_stf.stamp_ = ros::Time::now(); //assign the time stamp to current time;
// alternatively, could assign this to the OLDER of A or B transforms
return true; //if got here, the multiplication is valid
}
//fnc to print components of a transform
void DemoTfListener::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;
}
//fnc to print components of a stamped transform
void DemoTfListener::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 DemoTfListener::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;
}④ 创建一个主函数调用自定义的类
touch example_tf_listener.cpp
#include "example_tf_listener.h" //刚刚定义的类
using namespace std;
//在主函数进行位姿转换操作
int main(int argc, char** argv) {
ros::init(argc, argv, "demoTfListener"); //节点名称
ros::NodeHandle nh; // 创建一个句柄，通过它与构造函数通信
ROS_INFO("main: instantiating an object of type DemoTfListener");
DemoTfListener demoTfListener(&nh); //实例化一个类的对象并通过对主函数节点的句柄添加指针发送到构造函数处
tf::StampedTransform stfBaseToLink2, stfBaseToLink1, stfLink1ToLink2;//创建附带时间信息的转换对象
tf::StampedTransform testStfBaseToLink2;
tf::Transform tfBaseToLink1, tfLink1ToLink2, tfBaseToLink2, altTfBaseToLink2;//创建不附带时间信息的转换对象
//lookupTransform是自定义类头文件中的头文件里定义的函数，现成的API
//lookupTransform函数可以获得两个坐标系之间的转换矩阵
//第一个参数是target_frame ，第二个参数是source_frame ,第三个参数是评估source_frame的时间，设置为0意味着评估最新的时间 ,第四个是转换矩阵的名称，A to B
demoTfListener.tfListener_->lookupTransform("base_link", "link1", ros::Time(0), stfBaseToLink1);
cout << endl << "base to link1: " << endl;
demoTfListener.printStampedTf(stfBaseToLink1); //将结果输出到屏幕
tfBaseToLink1 = demoTfListener.get_tf_from_stamped_tf(stfBaseToLink1);
demoTfListener.tfListener_->lookupTransform("link1", "link2", ros::Time(0), stfLink1ToLink2);
cout << endl << "link1 to link2: " << endl;
demoTfListener.printStampedTf(stfLink1ToLink2);
tfLink1ToLink2 = demoTfListener.get_tf_from_stamped_tf(stfLink1ToLink2);
demoTfListener.tfListener_->lookupTransform("base_link", "link2", ros::Time(0), stfBaseToLink2);
cout << endl << "base to link2: " << endl;
demoTfListener.printStampedTf(stfBaseToLink2);
tfBaseToLink2 = demoTfListener.get_tf_from_stamped_tf(stfBaseToLink2);
cout << endl << "extracted tf: " << endl;
demoTfListener.printTf(tfBaseToLink2);
altTfBaseToLink2 = tfBaseToLink1*tfLink1ToLink2;
cout << endl << "result of multiply tfBaseToLink1*tfLink1ToLink2: " << endl;
demoTfListener.printTf(altTfBaseToLink2);
//以下函数是计算链坐标系的转换关系的，link1*link2,可以得到从base到link2的转换矩阵
if (demoTfListener.multiply_stamped_tfs(stfBaseToLink1, stfLink1ToLink2, testStfBaseToLink2)) {
cout << endl << "testStfBaseToLink2:" << endl;
demoTfListener.printStampedTf(testStfBaseToLink2);
}
cout << endl << "attempt multiply of stamped transforms in wrong order:" << endl;
demoTfListener.multiply_stamped_tfs(stfLink1ToLink2, stfBaseToLink1, testStfBaseToLink2);
geometry_msgs::PoseStamped stPose, stPose_wrt_base;
stPose = demoTfListener.get_pose_from_transform(stfLink1ToLink2);//从转换矩阵中提取位姿信息
cout << endl << "pose link2 w/rt link1, from stfLink1ToLink2" << endl;
demoTfListener.printStampedPose(stPose);
//transformPose也是头文件里定义的函数，现成的API
demoTfListener.tfListener_->transformPose("base_link", stPose, stPose_wrt_base);
cout << endl << "pose of link2 transformed to base frame:" << endl;
demoTfListener.printStampedPose(stPose_wrt_base);
return 0;
}⑤ 编辑CMakeLists文档
添加如下语句即可，注意，只有是通过catkin_simple生成的包才可以用这个指令，用catkin_make生成的包，还是老老实实加上各个依赖
cs_add_executable(example_tf_listener src/example_tf_listener.cpp src/example_tf_listener_fncs.cpp)最后通过catkin_make即可生成可执行文件，可以在gazebo中导入模型，需要注意的是，一定要在.xacro 文件中添加joint_state_publisher这个plugin，否则无法获取关节的位姿信息。