tf与坐标转换

一、发布tf

方法一：wiki上的方法，tf::Transform

void poseCallback(const turtlesim::PoseConstPtr& msg){
    static tf::TransformBroadcaster br;
    tf::Transform transform;
    transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
    tf::Quaternion q;
    q.setRPY(0, 0, msg->theta);
    transform.setRotation(q);
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), 
                    "world","base_link"));
}

方法二：geometry_msgs::TransformStamped

tf::TransformBroadcaster odom_broadcaster;
geometry_msgs::TransformStamped leg_odom_trans;
leg_odom_trans.header.stamp = ros::Time::now();
leg_odom_trans.header.frame_id = "odom";
leg_odom_trans.child_frame_id = "base_link";
leg_odom_trans.transform.translation.x = robot_state->pos_world[0];
leg_odom_trans.transform.translation.y = robot_state->pos_world[1];
leg_odom_trans.transform.translation.z = robot_state->pos_world[2];
auto q = tf::createQuaternionFromRPY(robot_state->rpy[0] / 180 * PI,
         robot_state->rpy[1] / 180 * PI, 
         robot_state->rpy[2] / 180 * PI);
tf::quaternionTFToMsg(q, leg_odom_trans.transform.rotation);
odom_broadcaster.sendTransform(leg_odom_trans);

备注：其中ros::Time::now()可换成真实的ros::Time时间戳。头文件需包含：

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

二、订阅tf

方法一：订阅截至当前最新的tf，不需要等待

tf::TransformListener listener;
tf::StampedTransform transform;
try{
    listener.lookupTransform("/turtle2", "/turtle1",  
                             ros::Time(0), transform);
}
catch (tf::TransformException ex){
    ROS_ERROR("%s",ex.what());
    ros::Duration(1.0).sleep();
}

方法二：订阅当前时刻的tf，需要等待

tf::TransformListener listener;
tf::StampedTransform transform;
try{
    listener.waitForTransform("/turtle2", "/turtle1",
                              ros::Time::now(), ros::Duration(3.0));
    listener.lookupTransform("/turtle2", "/turtle1",  
                             ros::Time::now(), transform);
}
catch (tf::TransformException ex){
    ROS_ERROR("%s",ex.what());
    ros::Duration(1.0).sleep();
}

备注：也可把ros::Time::now()改为ros::Time::now()-ros::Duration(1.0)订阅1秒前的tf，改为某个点云时间戳，用于点云坐标系转换。头文件需包含：

#include <ros/ros.h>
#include <tf/transform_listener.h>

三、点云坐标轴转换——订阅tf并转为eigen

tf::TransformListener listener;
bool transformPointCloud(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr pointCloud, pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloudTransformed, const std::string& targetFrame)
{
const std::string inputFrameId(pointCloud->header.frame_id);
try{
    listener.waitForTransform(targetFrame, inputFrameId, 
                                        timeStamp, ros::Duration(1.0));
    listener.lookupTransform(targetFrame, inputFrameId, 
                                       timeStamp, transformTf);
} 
catch (tf::TransformException &ex) {
    ROS_ERROR("%s", ex.what());
    return false;
}

Eigen::Affine3d transform;
poseTFToEigen(transformTf, transform);
pcl::transformPointCloud(*pointCloud, *pointCloudTransformed,         
                         transform.cast<float>());
pointCloudTransformed->header.frame_id = targetFrame;
}

备注：头文件需包含

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <tf_conversions/tf_eigen.h>
#include <Eigen/Core>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>

四、点云坐标转换——构造eigen

Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();
float theta = M_PI/4;   //旋转的度数，这里是45度
transform_1 (0,0) = cos (theta);  //这里是绕的Z轴旋转
transform_1 (0,1) = -sin(theta);
transform_1 (1,0) = sin (theta);
transform_1 (1,1) = cos (theta);
        
//transform_1 (0,2) = 0.3;   //这样会产生缩放效果
//transform_1 (1,2) = 0.6;
//transform_1 (2,2) = 1;
 
transform_1 (0,3) = 25; //这里沿X轴平移
transform_1 (1,3) = 30;
transform_1 (2,3) = 380;
pcl::PointCloud<pcl::PointXYZ>::Ptr transform_cloud1 (new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud(*cloud,*transform_cloud1,transform_1);

五、点的坐标轴转换

tf::TransformListener listener
geometry_msgs::PointStamped pointBodyFrame,pointOdomFrame;
pointBodyFrame.header.frame_id="base_link";
pointBodyFrame.header.stamp=ros::Time(0);
pointBodyFrame.point.x=1.0;   
pointBodyFrame.point.y=2.0;
pointBodyFrame.point.z=0;
try{ 
    listener_.transformPoint("odom",pointBodyFrame,pointOdomFrame);
}catch (tf::TransformException &ex) {
    ROS_ERROR("%s",ex.what());
}

备注：转换关系的时间用的是被转换点的时间戳。头文件需包含：

#include <ros/ros.h>
#include <tf/transform_listener.h>

使用点的时间戳对应的tf进行转换，ros::Time(0)表示使用最新的tf，不能用ros::Time::now()。