ros基础（TF管理）

Transform Configuration（变换配置）

假设我们知道，传感器是挂在机体中心的前方10cm，高度20cm处。这就等于给了我们一种转换的偏移关系。具体来说，就是，从传感器到机体的坐标转换关系应该为(x:0.1m,y:0.0m, z:0.2m),相反的转换即是(x:-0.1m,y:0.0m,z:0.2m)。
利用tf来管理这种关系，我们需要把他们添加到转换树（transform tree）中。一方面来说，转换树中的每一个节点都对应着一类坐标系，节点之间的连线即是两个坐标相互转换的一种表示，一种从当前节点到子节点的转换表示。Tf利用树结构的方式，保证了两个坐标系之间的只存在单一的转换，同时假设节点之间的连线指向是从parent到child。

基于例子，需要创建两个节点，一个“base_link”，一个是“base_laser”。

为了定义两者的关系，首先，需要决定谁是parent，谁是child?

由于tf假设所有的转换都是从parent到child的，因此谁是parent是有差别的。我们选择“base_link”坐标系作为parent，其他的传感器等，都是作为“器件”被添加进robot的，对于“base_link”和“base_laser”他们来说，是最适合的。这就意味着转换关系的表达式应该是(x:0.1m,y0.0m,z:0.2m)。关系的建立，在收到“base_laser”的数据到“base_link”的转换过程，就可以是简单的调用tf库即可完成。

Broadcasting a Transform（广播变换）

实现广播任务base_laser->base_link。 tf_broadcaster.cpp:

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

int main(int argc, char** argv){
  ros::init(argc, argv, "robot_tf_publisher");
  ros::NodeHandle n;

  ros::Rate r(100);

  tf::TransformBroadcaster broadcaster;

  while(n.ok()){
    broadcaster.sendTransform(
      tf::StampedTransform(
        tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
        ros::Time::now(),"base_link", "base_laser"));
        # 通过TransformBroadcaster来发送转换关系，需要附带5个参数。
        # 第1个参数，我们传递了旋转变换，在两个坐标系的发送的任意旋转，都必须通过调用btQuaternion.现在情况下，我们不想旋转，所以我们在调用btQauternion的时候，将pitch,roll,yaw的参数都置0.
        # 第2个参数，btVector3，任何变换过程都需要调用它。我们调用btVector3，相应的传感器的x方向距离机体基准偏移10cm，z方向20cm。
        # 第3个参数，我们需要给定转换关系携带一个时间戳，我们标记为ros::Time::now()。
        # 第4个参数，我们需要传递parent节点的名字。
        # 第5个参数，传递的是child节点的名字。
    r.sleep();
  }
}

Using a Transform（调用变换）

上面的节点发布l了转换关系，baser_laser->base_link。
现在，我们需要利用转换关系，将从传感器获取的数据转换到机体对应的数据，即是“base_laser”->到“base_link”坐标系的转换。
tf_listener.cpp：

#include <ros/ros.h>
#include <geometry_msgs/PointStamped.h>
#include <tf/transform_listener.h> 
//一个TransformListener目标会自动的订阅ROS系统中的变换消息主题，同时管理所有的该通道上的变换数据。

void transformPoint(const tf::TransformListener& listener){
  //we'll create a point in the base_laser frame that we'd like to transform to the base_link frame
  geometry_msgs::PointStamped laser_point; //创建一个虚拟点，作为geometry_msgs::PointStamped。
  laser_point.header.frame_id = "base_laser"; //对于header里的frame_id，设置为“base_laser”,因为创建的是雷达坐标系里的点。
  
  //we'll just use the most recent transform available for our simple example
  laser_point.header.stamp = ros::Time(); //设置时间戳为ros::time(),允许请求TransformListener取得最新的变换数据。

  //just an arbitrary point in space
  laser_point.point.x = 1.0;
  laser_point.point.y = 0.2;
  laser_point.point.z = 0.0;
  //最后，设置具体点，比如x:1.0,y:0.2,z:0.0


  try{
    geometry_msgs::PointStamped base_point;
    listener.transformPoint("base_link", laser_point, base_point);
	// 通过TransformListener对象，调用transformPoint(),填充三个参数来进行数据变换。
	// 第1个参数，代表想要变换到的目标坐标系。
	// 第2个参数填充需要变换的原始坐标系的点对象。
	// 第3个参数填充，目标坐标系的点对象。
	// 在函数调用后，base_point里存储的信息就是变换后的点坐标。


    ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",
        laser_point.point.x, laser_point.point.y, laser_point.point.z,
        base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());
  }
  catch(tf::TransformException& ex){
    ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what());
  }
}

int main(int argc, char** argv){
  ros::init(argc, argv, "robot_tf_listener");
  ros::NodeHandle n;

  tf::TransformListener listener(ros::Duration(10));

  //we'll transform a point once every second
  ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener)));

  ros::spin();

}

运行两个节点 ，打印的结果

[ INFO] 1248138528.200823000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138528.19
[ INFO] 1248138529.200820000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138529.19