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 <math.h>
#include <stdlib.h>
#include <string>
#include <vector>

#include <ros/ros.h> //必加

#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 <tf/LinearMath/QuadWord.h>


// 定义一个类，包括构造函数，成员变量和成员函数
class DemoTfListener
{
public:
    DemoTfListener(ros::NodeHandle* nodehandle); //主函数需要通过一个句柄来调用这里的构造函数

    geometry_msgs::PoseStamped get_pose_from_transform(tf::StampedTransform tf);//获取一个对象的位姿信息的成员函数

    bool multiply_stamped_tfs(tf::StampedTransform A_stf, tf::StampedTransform B_stf,tf::StampedTransform &C_stf);//通过这个成员函数可以转换同一个链坐标系中的各个位姿信息

    void printStampedTf(tf::StampedTransform sTf);    
    void printStampedPose(geometry_msgs::PoseStamped stPose);    
    void printTf(tf::Transform tf);

    tf::Transform get_tf_from_stamped_tf(tf::StampedTransform sTf);     
    //illustrates a tf_listener in a class; this is somewhat more complex than creating a tf_listener in main()
    tf::TransformListener* tfListener_; 

private:
    ros::NodeHandle nh_; // 通过这个句柄实现在主函数和构造函数之间的信息        
}; 

#endif  

③ 创建一个执行类中方法的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是自定义类头文件中的头文件<tf/transform_listener.h>里定义的函数，现成的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也是头文件<tf/transform_listener.h>里定义的函数，现成的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，否则无法获取关节的位姿信息。