【ROS学习】- tf学习 - tf中重要函数解析 (陆续更新....)

这里记录一些 ROS中的 tf 用到的一些重要函数解析，陆续更新…

一、函数waitForTransform()、lookupTransform()

// tf监听器
tf::TransformListener listener;
// 查找turtle2与turtle1的坐标变换
listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);

这里 waitForTransform() ， /turtle2 是target_frame，这里理解为 数据应转换到的frame, 也就是 tf 的 frame_id ； /turtle1 是source_frame ，这里理解为 数据来源的frame, 也就是 tf 的 child_frame_id 。 等待的是 /turtle2 到 /turtle1 的坐标转换，ros::Time(0) 代表最近时刻的有效数据，ros::time::now() 是现在这个时刻的有效数据，不可以把 ros::Time(0) 改成 ros::time::now() ，因为监听做不到实时，会有几毫秒的延迟；最长等待 3.0 s 的时间。

这里 lookupTransform() , /turtle2 是target_frame，这里理解为 数据应转换到的frame, 也就是 tf 的 frame_id ； /turtle1 是source_frame ，这里理解为 数据来源的frame, 也就是 tf 的 child_frame_id 。 等待的是 /turtle2 到 /turtle1 的坐标转换，ros::Time(0) 代表最近时刻的有效数据，ros::time::now() 是现在这个时刻的有效数据，不可以把 ros::Time(0) 改成 ros::time::now() ，因为监听做不到实时，会有几毫秒的延迟；将得到的转换结果存放到 transform 中。

具体解释

这里的 waitForTransform() 传入的第一个参数是 目标坐标系 , 第二个参数是 源坐标系。

waitForTransform() 原型是
（其函数原型可在这找到 ：传送门 http://docs.ros.org/jade/api/tf/html/c++/classtf_1_1Transformer.html#a72dc26fe7bfcb9585123309e092e5c83 ）

//  target_frame : 这里理解为 数据应转换到的frame, 也就是 tf 的 frame_id 
//  source_frame : 这里理解为 数据来源的frame,    也就是 tf 的 child_frame_id
bool Transformer::waitForTransform 	(
		const std::string &  	target_frame,
		const std::string &  	source_frame,
		const ros::Time &  	time,
		const ros::Duration &  	timeout,
		const ros::Duration &  	polling_sleep_duration = ros::Duration(0.01),
		std::string *  	error_msg = NULL 
	) 		const 	

同样，这里的 lookupTransform() 传入的第一个参数是 目标坐标系 , 第二个参数是 源坐标系。

lookupTransform() 原型是
（其函数原型可在这找到 ：传送门 hhttp://docs.ros.org/en/jade/api/tf/html/c++/classtf_1_1Transformer.html#ac01a9f8709a828c427f1a5faa0ced42b ）

//  target_frame : 这里理解为 数据应转换到的frame, 也就是 tf 的 frame_id 
//  source_frame : 这里理解为 数据来源的frame,    也就是 tf 的 child_frame_id
void Transformer::lookupTransform 	( 	
		const std::string &  	target_frame,
		const std::string &  	source_frame,
		const ros::Time &  	time,
		StampedTransform &  	transform 
	) 		const

二、函数 tf::StampedTransform()、sendTransform()

tf::TransformBroadcaster br;
tf::Transform transform;
  
transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));

这里的 tf::StampedTransform() 是一个构造函数，给数据赋值，传入的参数中 transform 是包含tf的具体数据（现在这个数据里面还不包含frame_id、frame_child_id）， turtle1 是父坐标系，carrot1 是子坐标系，时间戳是ros::Time::now() 。

具体解释

StampedTransform() 是一个构造函数， 原型是
（其函数原型可在这找到 ：传送门 http://docs.ros.org/jade/api/tf/html/c++/classtf_1_1StampedTransform.html )

tf::StampedTransform::StampedTransform 	( 	
		const tf::Transform &  	input,
		const ros::Time &  	timestamp,
		const std::string &  	frame_id,
		const std::string &  	child_frame_id 
	) 		[inline]

sendTransform() 的原型是
（其函数原型可在这找到 ：传送门 http://docs.ros.org/jade/api/tf/html/c++/classtf_1_1TransformBroadcaster.html#acea192abb3558dd1f53c3269f2922d14 )

注意这里有函数重载，这里调用的是第一个函数

void tf::TransformBroadcaster::sendTransform ( const StampedTransform &  transform	) 	

//Send a StampedTransform The stamped data structure includes frame_id, and time, and parent_id already.
//发送一个 StampedTransform（这是一个类） 类型的数据，这个数据包括父坐标系、子坐标系、时间戳

三、函数 Transformer::canTransform()

boost::shared_ptr<tf::TransformListener> listener;
if (!listener->canTransform(scan->header.frame_id, robot_frame, scan->header.stamp, &error_msg))
{
	ROS_ERROR_STREAM_THROTTLE(1.0, "can not transform: "<<robot_frame<<" to "<<scan->header.frame_id);
	return;
}

这里的 Transformer::canTransform() 作用是测试 tf 是否possible(可用)，传入的参数中scan->header.frame_id 是目标坐标系，robot_frame 是源坐标系，scan->header.stamp 是时间戳，error_msg 是会返回 一个表征 如果 转换(transform) 失败的原因 的标志数据。

具体解释

Transformer::canTransform() 函数原型：
（其函数原型可在这找到 ：传送门
(http://docs.ros.org/jade/api/tf/html/c++/classtf_1_1Transformer.html#aea15c33a1a24e55bd4a35b86b8423ee1)

bool Transformer::canTransform	(	const std::string & 	target_frame,
const std::string & 	source_frame,
const ros::Time & 	time,
std::string * 	error_msg = NULL 
)		const

四、函数 Transformer::transformPose ()、Transformer::transformQuaternion（）

（ Transformer::transformPose () 和 Transformer::transformQuaternion（）的使用方法和含义基本是一样的，这里只对 Transformer::transformPose () 做简单解释）

TransformListenerWrapper* tf_;
tf::Stamped<tf::Pose> ident (tf::Transform(tf::createIdentityQuaternion(),
       						tf::Vector3(0,0,0)),
                            ros::Time(), laser_scan->header.frame_id);
tf::Stamped<tf::Pose> laser_pose;
tf_->transformPose(base_frame_id_, ident, laser_pose);  

这里传入的参数：目标坐标系、ident 是输入的 带时间戳的位姿、laser_pose 是输出的 带时间戳的位姿。
该函数主要是 查找了一个一个目标坐标系base_frame_id_ 到 ident 中的源坐标系laser 之间的 tf， ident 的中的tf转换数据会被赋值到 laser_pose 中。

具体解释

Transformer::transformPose() 函数主要是 查找了一个目标坐标系target_frame到stamped_in中的源坐标系之间的 tf ， stamped_in 的中的tf转换数据会被赋值到 stamped_out 中。

Transformer::transformPose() 函数原型：
（其函数原型可在这找到 ： 传送门 ( http://docs.ros.org/en/jade/api/tf/html/c++/classtf_1_1Transformer.html#a0a7b72eb4cc62194164d030afbefda9a ) ）

注意这里有函数重载，这里调用的是第一个函数：

void Transformer::transformPose ( const std::string &  target_frame,
					const Stamped< tf::Pose > &  	stamped_in,
					Stamped< tf::Pose > &  	stamped_out 
	) 	const

//   target_frame	目标坐标系
//    stamped_in	输入 带时间戳的位姿
//    stamped_out	输出 带时间戳的位姿

注意 Transformer::transformPose() 函数内部调用了 lookupTransform() 函数，所以这部分实际上做了一个监听 tf 的作用。
函数的具体实现：

 void Transformer::transformPose(const std::string& target_frame, const Stamped<Pose>& stamped_in, Stamped<Pose>& stamped_out) const
{
   StampedTransform transform;
   lookupTransform(target_frame, stamped_in.frame_id_, stamped_in.stamp_, transform);
 
   stamped_out.setData(transform * stamped_in);
   stamped_out.stamp_ = transform.stamp_;
   stamped_out.frame_id_ = target_frame;
};

由于函数重载，这个函数的第二个实现方法是：

void Transformer::transformPose(const std::string& target_frame, const ros::Time& target_time,
                                 const Stamped<Pose>& stamped_in,
                                 const std::string& fixed_frame,
                                 Stamped<Pose>& stamped_out) const
{
   StampedTransform transform;
   lookupTransform(target_frame, target_time,
                   stamped_in.frame_id_,stamped_in.stamp_,
                   fixed_frame, transform);
 
   stamped_out.setData(transform * stamped_in);
   stamped_out.stamp_ = transform.stamp_;
   stamped_out.frame_id_ = target_frame;
};

五、函数 pcl_ros::transformPointCloud()

pcl_ros::transformPointCloud("/velodyne", *input_cloud, *output_cloud, *tran);

其中第一个参数为目标坐标系，第二个参数为原始点云，第三个参数为 目标点云，
第四个参数为接收到的坐标，正变换、逆变换 都包含其中

具体解释

pcl_ros::transformPointCloud() 函数原型：
（其函数原型可在这找到 ： 传送门 ( http://docs.ros.org/jade/api/pcl_ros/html/namespacepcl__ros.html#aad20af010640a41f1898e75f6a6121ad ) ）

注意这里有函数重载，这里调用的是第二个函数

template<typename PointT >
bool pcl_ros::transformPointCloud 	( 	
		const std::string &  	target_frame,
		const pcl::PointCloud< PointT > &  	cloud_in,
		pcl::PointCloud< PointT > &  	cloud_out,
		const tf::TransformListener &  	tf_listener 
	) 		

//Transforms a point cloud in a given target TF frame using a TransformListener.//把点云转换到给定的目标坐标系下

//参数:
//    target_frame	目标坐标系
//    cloud_in	输入点云
//    cloud_out	输出点云
//    tf_listener	 tf监听者对象