参考:
http://wiki.ros.org/tf#static_transform_publisher
https://blog.csdn.net/qit1314/article/details/83280992
https://www.guyuehome.com/19226
一、launch中Tf的解析
对于这个x y z yaw pitch roll是怎么解析的?
基本可以理解为:
将子坐标系移动到parent坐标系的(x,y,z)位置下,然后子坐标系绕自己的动轴旋转,按照ZYX的顺序。
绕动轴旋转,在参数设置上很不直观啊!
比如:
<launch>
<!--- TF -->
<node pkg="tf" type="static_transform_publisher" name="base_link1_to_camera" args="1 0 0 1.570795 0 0 /camera /b1 10" />
<node pkg="tf" type="static_transform_publisher" name="base_link2_to_camera" args="2 0 0 1.570795 1.570795 0 /camera /b2 10" />
<node pkg="tf" type="static_transform_publisher" name="base_link3_to_camera" args="3 0 0 1.570795 1.570795 1.570795 /camera /b3 10" />
<node pkg="tf" type="static_transform_publisher" name="base_link4_to_camera" args="4 0 0 1.570795 1.570795 -1.570795 /camera /b4 10" />
</launch>
以上设定了4个相对与camera的子坐标系:
b1:在(1,0,0)处,绕Z轴(自己的)逆时针旋转了90度(右手握螺旋,大拇指朝向轴方向,四指的朝向为逆时针)
b2:在(2,0,0)处,先Z轴旋转90度后,绕新的Y轴旋转90度;
b3:在(3,0,0)处,先Z轴旋转90度后,绕新的Y轴旋转90度,再绕新的X轴旋转90度;
b4:在(4,0,0)处,先Z轴旋转90度后,绕新的Y轴旋转90度,再绕新的X轴旋转-90度;
可以对比着看:
b1对比camera坐标轴,
b2对比b1,
b3和b4对比b2.
红色是X轴,绿色是Y轴,蓝色是Z轴。
二、setRPY的测试:
这个是绕定轴旋转的,绕x轴,再绕y轴,再绕z轴,所绕的轴是一直不变的。
如果要自己在代码里调用setRPY得到对应的四元数,一定要注意传参的顺序,因为setRPY接收的参数的顺序为roll,pitch,yall,从其命名也可以看出这一点。
quat.setRPY(1.570795,0 ,0)得到的结果:
quat.setRPY(1.570795,1.570795 ,0)得到的结果:
在上面的基础上,绕着固定轴Y轴(camera系的y轴)旋转90度:
quat.setRPY(1.570795,1.570795 ,1.570795)得到的结果:
在上面的基础上,绕着固定轴Z轴(camera系的z轴)旋转90度:
还可以调用setEulerZYX这个方法来得到对应的四元数。
这个方法是从另外一个角度去看参数的(这个是绕动轴旋转),就是先绕Z,再绕新的Y,再绕新的X,这个对应角度的命名刚好就是yaw,pitch,roll。
这个方法和绕定轴旋转的setRPY方法得到的结果是一样的。
还有一个setEuler:
2 setEuler
这种方式是绕着动轴转动,先绕Y轴,在绕变换后的X轴,再绕变换后的Z轴旋转。
从数学形式上说,这是绕定轴YXZ矩阵依次右乘,即:R = R(y)R(x)R(z) 的顺序。
/**@brief Set the quaternion using Euler angles
@param yaw Angle around Y
@param pitch Angle around X
@param roll Angle around Z */
void setEuler(const tfScalar& yaw, const tfScalar& pitch, const tfScalar& roll);
带Euler都是动轴。
以上得到的四元数,表示是的子坐标相对于parent坐标的旋转(想象一下我们刚才先把子坐标移动到位置后,再旋转)。
因此,子坐标的点,用这个四元数乘上以后,加上偏移值就得到了parent坐标系下的表示。
验证方法:
参考上面链接,写个ros工程,打开rviz,添加TF看结果。