ros环境下，tf::tfMessage的读取，使用，读取kitti数据集groundtruth显示

读取kitti数据集并显示groundtruth是检验自己slam算法的必须品。

读取rosbag里的tf::tfMessage就是groundtruth。

数据下载：

链接: https://pan.baidu.com/s/1JD2G4f29pyn--x5RBMpYkg 提取码: hx9y

下载后使用下面代码进行合并之后解压（百度云盘不让上传4G以上的文件。。）

cat 2011_09_30_0027.bag.tar.gz* > 2011_09_30_0027.bag.tar.gz

 可以使用info读取到包内的数据结构第一个就是tf::tfMessage

读取tf::tfMessage数据需要在CMakelist和package.xml里添加tf

find_package(catkin REQUIRED COMPONENTS
  tf
)

<build_depend>tf</build_depend>

在sub数据时注意名字。

ros::Subscriber tf_sub = n.subscribe("/tf", 100, tf_Callhandle);

在会掉函数时注意结构。tansforms是一个vector，第0号是正确位姿，后面都没用。具体结构可以看tf/tfMessage 和 tf 树 理解

nav_msgs::Path groundPath;
void tf_Callhandle(const tf::tfMessage msg_tf)
{
    //输出轨迹
    nav_msgs::Odometry laserOdometry;
    laserOdometry.header.frame_id = "map";
    laserOdometry.child_frame_id = "map_child";
    laserOdometry.header.stamp = msg_tf.transforms[0].header.stamp;
    laserOdometry.pose.pose.orientation.x = msg_tf.transforms[0].transform.rotation.x;
    laserOdometry.pose.pose.orientation.y = msg_tf.transforms[0].transform.rotation.y;
    laserOdometry.pose.pose.orientation.z = msg_tf.transforms[0].transform.rotation.z;
    laserOdometry.pose.pose.orientation.w = msg_tf.transforms[0].transform.rotation.w;
    laserOdometry.pose.pose.position.x = msg_tf.transforms[0].transform.translation.x;
    laserOdometry.pose.pose.position.y = msg_tf.transforms[0].transform.translation.y;
    laserOdometry.pose.pose.position.z = msg_tf.transforms[0].transform.translation.z;

    geometry_msgs::PoseStamped laserPose;
    laserPose.header = laserOdometry.header;
    laserPose.pose = laserOdometry.pose.pose;
    groundPath.header.stamp = laserOdometry.header.stamp;
    groundPath.poses.push_back(laserPose);
    groundPath.header.frame_id = "map";
    pubgroundPath.publish(groundPath); 
}