tf最简单易懂的demo

新建一个tf发布者

//zq_broadcaster.cpp
#include <ros/ros.h>		//编写ros程序所必须的
#include <tf/transform_broadcaster.h>		//基于tf库
#include <turtlesim/Pose.h>		//turtlesim功能包，乌龟位姿信息获取（机关它与世界坐标转换关系）


int main(int argc,char** argv)
{
	//查看argc和argv，执行命令行提供的任何ROS參數和名称重映射		
	ros::init(argc,argv,"zq_broadcaster");	
	ros::NodeHandle node;
	ros::Rate loop_rate(10);

	
    while (ros::ok())
    {
	    static tf::TransformBroadcaster br;		//发送坐标转换
	    tf::Transform transform;	//2D转换为3D
	    transform.setOrigin( tf::Vector3(2.0, 1.0, 0.0) );
	    tf::Quaternion q;
	    q.setRPY(0, 0, 1.5); //单位是弧度单位,正:逆时针旋转
	    transform.setRotation(q);
	    //transform;时间戳;父框架名称(world);子框架名称(turtle)；
	    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "turtle"));
	    ROS_INFO("pub world, turtle tf");
	    loop_rate.sleep();
	}

	return 0;
}

发布的坐标转换(在发布后最好用rviz查看是否正确)

订阅这个tf变换

//turtle_tf_listener.cpp
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
//#include <geometry_msgs/Twist.h>    //将监听到的消息转换为速度消息
//#include <turtlesim/Spawn.h>    //在同一窗口生成另一个乌龟turtle2(spawn再生服务)

int main(int argc, char** argv){
  ros::init(argc, argv, "zq_listener");

  ros::NodeHandle node;
  

  //创建监听器,用于监听是否存在需要的tf
  tf::TransformListener listener;
/*
*监听器查找特定的tf
*根据查找的tf信息计算相应的速度信息
*将速度信息发布给turtle2
*/
  ros::Rate rate(10.0);


  geometry_msgs::PointStamped world;
  world.header.stamp=ros::Time();
  world.header.frame_id="world";
  world.point.x=1;
  world.point.y=2;
  world.point.z=3;
  geometry_msgs::PointStamped turtle;
  


  while (node.ok())
  {
    tf::StampedTransform transform;
    try
    {
      /* 查找特定的tf
      *"/turtle2", "/turtle1":从turtle2到turtle1的转换(frame name)
      *ros::Time(0):指定tf时间,获取到最近存在的tf
      *transform:保存查找到的tf
      */
      listener.waitForTransform("/world", "/turtle", ros::Time(0), ros::Duration(3.0));
      listener.lookupTransform("/world", "/turtle",ros::Time(0), transform);
    }
    catch (tf::TransformException &ex) 
    {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
      continue;
    }

    ROS_INFO("x,y,z = %f,%f,%f",transform.getOrigin().x(),transform.getOrigin().y(),transform.getOrigin().z());
    ROS_INFO("w,y = %f,%f",transform.getRotation().getW(),transform.getRotation().getY());

    //transformPoint
    try
    {
      listener.waitForTransform("/world", "/turtle", ros::Time(0), ros::Duration(3.0));
      listener.transformPoint("/turtle",world,turtle);
      
    }
    catch (tf::TransformException &ex) 
    {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
    }
    ROS_INFO("turtle.point.x=%f,turtle.point.y=%f,turtle.point.z = %f",turtle.point.x,turtle.point.y,turtle.point.z);

    rate.sleep();
  }


  return 0;
};

输出结果

tf发布者输出

tf订阅者输出

注意事项

1 , tf监听时可监听两种坐标系的相互转化;
2 , listener.lookupTransform("/world", “/turtle”,ros::Time(0), transform);是指world坐标系里的坐标在,turtle坐标系里的表示;
3 , ros::Time(0):指定tf时间,获取到最近存在的tf

具体代码包详见:gitee-----learning_tf