Navigation导航包的学习实践-（一）TF坐标变换

配置tf--坐标系间的指定变换

• 参考ROS_WIKI

---

--->假定，我们以上层来描述“base_laser”坐标系的点，来转换到"base_link"坐标系。首先，我们需要创建节点，来发布转换关系到ROS系统中。下一步，我们必须创建一个节点，来监听需要转换的数据，同时获取并转换。在某个目录创建一个源码包，同时命名“robot_setup_tf”。添加依赖包roscpp,tf,geometry_msgs。

其中，roscpp是c++所需的功能包，geometry_msgs是发布PointStamped消息类型用到的功能包。
下面代码作用为：发布两个坐标系间的指定变换：

#include //发布的头文件
//我们创建一个TransformBroadcaster对象，之后我们可以利用他来发送变换关系，即base_link → base_laser
tf::TransformBroadcaster broadcaster;
//通过TransformBroadcaster来发送转换关系，需要附带5个参数,第一个参数是四元数转角，第二个参数是平移，第三个参数是时间戳，代表当期时间，第四个参数是parent节点的名字，第五个参数是child节点的名字。
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"));

其中Transform为发布的平移与旋转变换，如果存在旋转变换，需要转化为四元数，因此完整的程序应该如下所示：

tf:Transform transform;
transform.setOrigin( tf::Vector3(0.1, 0.0, 0.2) );
tf::Quaternion q;
q.setRPY(0, 0, 0);
transform.setRotation(q);
broadcaster.sendTransform( tf::StampedTransform(transform,ros::Time::now(),"base_link", "base_laser") );

创建好了变换关系（静态变化），下一步就是发布原始坐标数据，然后获取变换后的坐标数据。

#include 
#include  //监听变换的头文件
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;
   laser_point.header.frame_id = "base_laser";
 
   //we'll just use the most recent transform available for our simple example
   laser_point.header.stamp = ros::Time();
 
   //just an arbitrary point in space
   laser_point.point.x = 1.0;
   laser_point.point.y = 0.2;
   laser_point.point.z = 0.0;
 
   try{
     geometry_msgs::PointStamped base_point;
     listener.transformPoint("base_link", laser_point, 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();
 
 }

首先明确几个对象变量：

• tf::TransformListener - 创建该对象变量后，该变量会自动订阅ROS系统中的变换消息主题，同时管理所有通道上的变换数据，但是得写明变换的父、子坐标系！
• geometry_msgs::PointStamped - 这是个消息类型，必须引用相应的头文件。该消息类型包含有坐标系id,时间戳（Stamped），以及空间三维坐标点。这里将"base_laser"坐标定义为固定的坐标点。
• ros::Timer - 该类用来定义定时器对象，在程序中实现了每一秒钟进入transformPoint函数计算tf坐标变换。