使用tf库获取机器人位姿c++实现

使用tf库获取机器人位姿可以通过以下c++代码实现：

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

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

  tf::TransformListener listener;

  ros::Rate rate(10.0);
  while (node.ok()){
    tf::StampedTransform transform;
    try{
      listener.lookupTransform("/map", "/base_link", ros::Time(0), transform); // 获取机器人base_link相对于地图map的位姿信息
    }
    catch (tf::TransformException &ex) {
      ROS_ERROR("%s",ex.what());
    }

    ROS_INFO("Translation: x = %f, y = %f, z = %f", transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z()); // 打印机器人的位置信息
    ROS_INFO("Rotation: x = %f, y = %f, z = %f, w = %f", transform.getRotation().x(), transform.getRotation().y(), transform.getRotation().z(), transform.getRotation().w()); // 打印机器人的姿态信息

    rate.sleep();
  }
  return 0;
}

以上代码中，我们使用tf::TransformListener类来监听tf变换，并使用lookupTransform函数获取机器人base_link相对于地图map的位姿信息。其中，"/map"和"/base_link"分别表示地图和机器人的坐标系，ros::Time(0)表示获取最近的一次tf变换信息。最后，我们通过transform.getOrigin()和transform.getRotation()函数获取机器人的位置和姿态信息，并打印输出。