MAVROS PX4外部坐标系输入与内部坐标系对齐

MAVROS PX4外部坐标系输入与内部坐标系对齐

有一个激光雷达，安装在无人机顶部，其坐标轴朝向相对于无人机为右-前-上

一、QGC调整内部参数

1. 调整EKF和HGT为外部视觉输入；
2. 调整延时EKF2_EV_DELAY为一个合适的值，例如20ms。
   

二、写转换脚本

1. 转换脚本部分代码：

vehicle_type = sys.argv[1]
vehicle_id = sys.argv[2]
local_pose = PoseStamped()
local_pose.header.frame_id = 'world'
quaternion = tf.transformations.quaternion_from_euler(0, -math.pi/2, math.pi/2)
q = Quaternion([quaternion[3],quaternion[0],quaternion[1],quaternion[2]])def vins_callback(data):    local_pose.pose.position.x = data.pose.pose.position.xlocal_pose.pose.position.y = data.pose.pose.position.ylocal_pose.pose.position.z = data.pose.pose.position.zq_= Quaternion([data.pose.pose.orientation.w,data.pose.pose.orientation.x,data.pose.pose.orientation.y,data.pose.pose.orientation.z])q_ = q_*qlocal_pose.pose.orientation.w = q_[0]local_pose.pose.orientation.x = q_[1]local_pose.pose.orientation.y = q_[2]local_pose.pose.orientation.z = q_[3]

2. 坐标系xyz转换：由于激光雷达是RFU，需要转换成FLU，因此需要y幅值给x，-x幅值给y，z不改变。
3. 旋转角转换：旋转角的坐标系似乎比较奇怪，是RFU的，正好与激光雷达一致，因此角度转换不需要，最终修改的部分代码如下：

quaternion = tf.transformations.quaternion_from_euler(0, 0, 0)def vins_callback(data):    local_pose.pose.position.x = data.pose.pose.position.ylocal_pose.pose.position.y = -data.pose.pose.position.xlocal_pose.pose.position.z = data.pose.pose.position.z