世界坐标系(ENU)
ENU东北天坐标系,X朝东、Y朝北、Z朝天。
从 CAN读取RTK数据,解析得到X、Y和偏航角Yaw;Yaw为车体当前朝向与正东方向的夹角(X轴)。
车体坐标系
右手坐标系,在车体视角,X轴朝正前,Y轴朝右,Z轴朝上
坐标转换
br = tf.TransformBroadcaster()
br.sendTransform((east_current,north_current,0),tf.transformations.quaternion_from_euler(0, 0, yaw_rad(yaw_trans-90)), rospy.Time.now(),"base_link", "map")
yaw_raw函数:将角度转为弧度