tf 使用_人非人1991的博客-CSDN博客
一、TF中的数据格式:
这些数据格式全都是class
头文件:#include <tf/transform_datatypes.h>基本上可以包含所有的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轴)
已知q,获取roll,pitch,yaw
tf::Matrix3x3(q).getRPY(roll,pitch,yaw);
3)tf_q = tf::createQuaternionFromYaw( theta ); theta是绕z轴旋转角,其他两个轴旋转角为0;
tf_q = tf::createQuaternionFromRollPitchYaw(roll,pitch,yaw);
如要生成ros_q,可以直接使用 :ros_q = tf::createQuaternionMsgFromYaw(theta);
ros_q = tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
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::Transform作为父类继承而来的
tf::StampedTransform stamped_transform
stamped_transform = tf::StampedTransform( transform, ros::Time::now(), "frame", "child_frame"); //使用参数初始化得到
可以直接获取stamped_transform.getRotation(), stamped_transform.getOrigin();
二、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
tf::StampedTransform stamped_transform; //定义存放变换关系的变量
try
{
//得到child_frame坐标系原点,在frame坐标系下的坐标与旋转角度
listener.lookupTransform("frame", "child_frame",ros::Time(0), stamped_transform);
}
catch (tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
获取到stamped_transform后可以使用这些信息
transform.getOrigin().x()
transform.getOrigin().y()
transform.getRotation().getW();
transform.getRotation().getX();
transform.getRotation().getY();
transform.getRotation().getZ();
(2)transformPoint()
在实际应用中需要将一个坐标系下的点转化到另一个坐标系下
//将child_frame坐标系下的Point1,转化到frame坐标系下,存储在point2中
geometry_msgs::PointStamped point1;
point1.header.stamp=ros::Time();
point1.header.frame_id="child_frame";
point1.point.x=1;
point1.point.y=2;
point1.point.z=3;
geometry_msgs::PointStamped point2;
try
{
listener.transformPoint("frame",point1,point2);
}
catch (tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
四、四元数转欧拉角
/*********四元数转欧拉角********/
tf::Quaternion tf_q;
tf::quaternionMsgToTF(amcl_pose.pose.pose.orientation,tf_q);
double roll,pitch,yaw;
tf::Matrix3x3(tf_q).getRPY(roll,pitch,yaw);
//四元数转yaw角
tf::getYaw(tf_q);
tf::getYaw(ros_q);
/********欧拉角转四元数*****/
ros_q = tf::createQuaternionMsgFromYaw(yaw);
rt_q = f::createQuaternionFromYaw(yaw);
ros_q = tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
tf_q = tf::createQuaternionFromRollPitchYaw(roll,pitch,yaw);
tf_q.setRPY(roll,pitch,yaw);
void GetXYZWFromRPY(double roll, double pitch, double yaw, double& x, double& y, double& z, double& w)
{
roll /= 2;
pitch /= 2;
yaw /= 2;
x = sin(roll) * cos(pitch) * cos(yaw) - cos(roll) * sin(pitch) * sin(yaw);
y = cos(roll) * sin(pitch) * cos(yaw) + sin(roll) * cos(pitch) * sin(yaw);
z = cos(roll) * cos(pitch) * sin(yaw) - sin(roll) * sin(pitch) * cos(yaw);
w = cos(roll) * cos(pitch) * cos(yaw) + sin(roll) * sin(pitch) * sin(yaw);
}
五、已知tf::Transform tf 和其中一个坐标系下的 pose 可以将其转化到另一个坐标系下
geometry_msgs::Pose transformPose(geometry_msgs::Pose &pose, tf::Transform &tf)
{
tf::Pose tf_pose;
tf::poseMsgToTF(pose,tf_pose);
tf_pose = tf * tf_pose;
geometry_msgs::Pose ros_pose;
tf::poseTFToMsg(tf_pose, ros_pose);
return ros_pose;
}
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)