TF内部公式详解

　　　　　　　　　　　　　　　　　　　红色为ｘ,绿色为ｙ,蓝色为ｚ

发布如上转换

#include <ros/ros.h>		//编写ros程序所必须的
#include <tf/transform_broadcaster.h>		//基于tf库
const int pi = 3.1415926;
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((-1.0), 3.0, 0.0) );
	    tf::Quaternion q;
	    q.setRPY(0, 0, 0.5);
	    transform.setRotation(q);
	    //transform;时间戳;父框架名称(enu);子框架名称(park_loc)；
	    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "enu", "park_loc"));
	    ROS_INFO("pub enu, park_loc tf");
	    loop_rate.sleep();
	}

	return 0;
}

验证内部公式

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
//#include <geometry_msgs/Twist.h>    //将监听到的消息转换为速度消息
//#include <park_locsim/Spawn.h>    //在同一窗口生成另一个乌龟park_loc2(spawn再生服务)
const int pi = 3.1415926;
int main(int argc, char** argv){
  ros::init(argc, argv, "zq_listener");

  ros::NodeHandle node;
  

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


  geometry_msgs::PointStamped enu;
  enu.header.stamp=ros::Time();
  enu.header.frame_id="enu";
  enu.point.x=1;
  enu.point.y=2;
  enu.point.z=0;
  geometry_msgs::PointStamped park_loc;
  
  park_loc.point.x = cos(0.5)*(enu.point.x + 1)+sin(0.5)*(enu.point.y-3) ;
  park_loc.point.y = cos(0.5)*(enu.point.y-3)-sin(0.5)*(enu.point.x+1) ;
  ROS_ERROR("GXY:park_loc.point.x=%f,park_loc.point.y=%f,park_loc.point.z = %f",park_loc.point.x,park_loc.point.y,park_loc.point.z);

    

  while (node.ok())
  {
    tf::StampedTransform transform;
    try
    {
      /* 查找特定的tf
      *"/park_loc2", "/park_loc1":从park_loc2到park_loc1的转换(frame name)
      *ros::Time(0):指定tf时间,获取到最近存在的tf
      *transform:保存查找到的tf
      */
      listener.waitForTransform("/enu", "/park_loc", ros::Time(0), ros::Duration(3.0));
      listener.lookupTransform("/enu", "/park_loc",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("/park_loc", "/enu", ros::Time(0), ros::Duration(3.0));
      listener.transformPoint("/park_loc",enu,park_loc);
      
    }
    catch (tf::TransformException &ex) 
    {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
    }
    ROS_ERROR("TF:park_loc.point.x=%f,park_loc.point.y=%f,park_loc.point.z = %f",park_loc.point.x,park_loc.point.y,park_loc.point.z);
    rate.sleep();
  }
  return 0;
};

内部公式如下

  park_loc.point.x = cos(0.5)*(enu.point.x + 1)+sin(0.5)*(enu.point.y-3) ;
  park_loc.point.y = cos(0.5)*(enu.point.y-3)-sin(0.5)*(enu.point.x+1) ;

注意

1. 逆时针旋转到目标坐标系的角度
2. 在原坐标系的ｘ,ｙ坐标上进行先平移后旋转操作（tf为先平移后旋转）
   注意：先旋转后平移，和先平移后旋转完全不同，见下图
   
3. 发布TF后，一定要用rviz看下坐标转换是否是自己想要的！！！！！！！！！！！！！！！