ROS-TF库-写一个TransformBroadcaster发布

1 TransformBroadcaster类

• tf包提供了TransformBroadcaster类的实现，以帮助简化tf发布转换的任务。
• 要使用TransformBroadcaster，我们需要包含头文件。
  如下是发布一个world-turtle1的一个坐标变换,来具体看TransformBroadcaster类的用法:

#include 
#include 
#include 
std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr& msg){

  // 1. 创建一个tf发布对象
  static tf::TransformBroadcaster br;

  // 2. 创建一个tf对象
  tf::Transform transform;
　　
  // 3. 设置tf的平移旋转，（把小龟的２ｄ位姿复制到３ｄ变换）
  transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
  tf::Quaternion q;
  q.setRPY(0, 0, msg->theta);
  transform.setRotation(q);

  // 4. 发布tf变换
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

int main(int argc, char** argv){
  ros::init(argc, argv, "turtle_tf_broadcaster");
  if (argc != 2)
  {ROS_ERROR("need turtle name as argument"); return -1; };
  turtle_name = argv[1];
  ros::NodeHandle node;
  ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
  ros::spin();
  return 0;
};

2 运行发布

• 程序依赖/turtle_pose话题,启动一个turtlesim后再运行此程序,结果如下.
  

• 显示ｔｆ树
  　rosrun rqt_tf_tree rqt_tf_tree

• 输出两个坐标系的ｔｆ
  　rosrun tf tf_echo /co1 /co2