一、发布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()。