ros基础（TF管理）

实现广播任务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个参数来发送转换关系。         # 在第一个参数中，我们传递了旋转变换，必须调用两个坐标系发送的任意旋转btQuaternion.现在我们不想旋转，所以我们在调用btQauternion的时候，将pitch,roll,yaw的参数都置0.         # 第2个参数，btVector3.需要调用任何变换过程。我们调用它btVector3.相应传感器的x方向与机体基准偏差10cm，z方向20cm。         # 在第三个参数中，我们需要携带一个时间戳来确定转换关系。我们标记为ros::Time::now()。         # 我们需要传递第四个参数parent节点的名字。         # 第五个参数传递child节点名称。     r.sleep();   } } 

上述节点发布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