转:ROS-TF的使用(常用功能)

https://blog.csdn.net/u010876294/article/details/75004903

一、TF中的数据格式:

这些数据格式全都是class

头文件:#include 基本上可以包含所有的tf数据类型

(1)tf::Vector3

tf::Vector3 point;

默认构造一个对象:tf::Vector3(float x, float y, float z)

对于tf::Vector3对象还有set与get方法。

1)举例:构造一个xyz分别为0,0,0的对象  tf::Vector3(0,0,0)

2)举例:使用geometry_msgs::PoseStampedPtr pose构造一个对象

tf::Vector3(pose->pose.position.x,pose->pose.position.y,pose->pose.position.z)

 

(2)tf::Quaternion

tf::Quaternion q;

1) 举例:使用geometry_msgs::PoseStampedPtr pose构造一个对象

tf::quaternionMsgToTF(pose->pose.orientation,q);   //把geomsg形式的四元数转化为tf形式,得到q

2) q.setRPY(Rtheta,Ptheta,Ytheta);//根据已经知道的欧拉角进行设置q,分别是child_frame绕着frame坐标系下的roll(绕X轴),pitch(绕Y轴),yaw(绕Z轴)

3)tf_q = tf::createQuaternionFromYaw( theta ); theta是绕z轴旋转角,其他两个轴旋转角为0;

如要生成ros_q,可以直接使用 :ros_q = tf::createQuaternionMsgFromYaw(theta);

4) q = tf::Quaternion(0,0,0,1);//直接通过参数设置q,0,0,0,1代表没有任何旋转

拓展:把tf四元数转化为geomsg四元数

geometry_msg::Quaternion msg_q;

tf::quaternionTFToMsg(tf_q,msg_q);

 

(3)tf::Transform

tf::Transform transform

1) transform = tf::Transform( tf::Quaternion(0,0,0,1) , tf::Vector3(0,0,0) );   //使用参数初始化,得到transform

2) transform. setRotation( tf_q );

transform.setOrigin( tf_point );    //通过set方法得到transform

 

(4)tf::StampedTransform

tf::StampedTransform stamped_transform

stamped_transform = tf::Transform( transform, ros::Time::now(), "frame", "child_frame");     //使用参数初始化得到


二、tf::TransformBroadcaster

发布TF

头文件:#include< tf / transform_broadcaster.h  >

tf::TransformBroadcaster broadcaster;    //定义发布器

broadcaster.sendTransform( stamped_transform ) ;   //发布一个tf,stamped_transform是一个tf::StampedTransform类型的数据。

三、tf::TransformListener

头文件:#include< tf / transform_listener.h>

tf::TransformListener  listener   //定义监听器     
(1)lookuoTransform();   //获取tf


   
   
   
   
  1. tf::StampedTransform stamped_transform; //定义存放变换关系的变量
  2. try
  3. {
  4. //得到child_frame坐标系原点,在frame坐标系下的坐标与旋转角度
  5. listener.lookupTransform( "frame", "child_frame",ros::Time( 0), stamped_transform);
  6. }
  7. catch (tf::TransformException &ex)
  8. {
  9. ROS_ERROR( "%s",ex.what());
  10. ros::Duration( 1.0).sleep();
  11. continue;
  12. }

获取到stamped_transform后可以使用这些信息


   
   
   
   
  1. transform .getOrigin() .x()
  2. transform .getOrigin() .y()
  3. transform .getRotation() .getW();
  4. transform .getRotation() .getX();
  5. transform .getRotation() .getY();
  6. transform .getRotation() .getZ();

(2)transformPoint()

在实际应用中需要将一个坐标系下的点转化到另一个坐标系下


   
   
   
   
  1. //将child_frame坐标系下的Point1,转化到frame坐标系下,存储在point2中
  2. geometry_msgs::PointStamped point1;
  3. point1.header.stamp=ros::Time();
  4. point1.header.frame_id= "child_frame";
  5. point1.point.x= 1;
  6. point1.point.y= 2;
  7. point1.point.z= 3;
  8. geometry_msgs::PointStamped point2;
  9. try
  10. {
  11. listener.transformPoint( "frame",point1,point2);
  12. }
  13. catch (tf::TransformException &ex)
  14. {
  15. ROS_ERROR( "%s",ex.what());
  16. ros::Duration( 1.0).sleep();
  17. }

四、四元数转欧拉角


   
   
   
   
  1. tf::Quaternion tf_quat;
  2. tf::quaternionMsgToTF(amcl_pose.pose.pose.orientation,tf_quat);
  3. double roll,pitch,yaw;
  4. tf::Matrix3x3(tf_quat).getRPY(roll,pitch,yaw);
  5. 好像可以直接 tf::getYaw(tf_quat);
  6. 或者 tf::getYaw(amcl_pose.pose.pose.orientation);
  7. //括号里面可以为ROS四元数或者tf四元数
  8. tf::createQuaternionMsgFromYaw(yaw);
  9. tf::getYaw(msg->pose.pose.orientation);

五、已知tf::Transform tf 和其中一个坐标系下的 pose 可以将其转化到另一个坐标系下


   
   
   
   
  1. geometry_msgs::Pose transformPose(geometry_msgs::Pose &pose, tf::Transform &tf)
  2. {
  3. tf::Pose tf_pose;
  4. tf::poseMsgToTF(pose,tf_pose);
  5. tf_pose = tf * tf_pose;
  6. geometry_msgs::Pose ros_pose;
  7. tf::poseTFToMsg(tf_pose, ros_pose);
  8. return ros_pose;
  9. }

 

你可能感兴趣的:(ros,tf,坐标转换)