PX4外部定位（T265/MOCAP/UWB等）配置记录_px4 外部数据

版本说明：

实验采用PX4 1.14固件版本+ROS1

0 引言

PX4系统使用NED作为参考坐标系（全局坐标系），如果使用方向朝前的磁力计，NED（X轴-North，Y轴-Earth，Z轴-Down）。

ROS系统中，当我们获取到外部定位数据时，一般可以通过/mavros/odometry/out(type: nav_msgs/Odometry)或者/mavros/vision_pose/pose（type: geometry_msgs/PoseStamped）中，需要注意的是，前者发送话题类型nav_msgs/Odometry需要设定/frame_id和/child_frame_id分别为/odom和/base_link才能直接发送给px4（不设定任何tf树的话），当外部定位装置机体坐标系与PX4机体坐标系存在偏差时，需要提供转换，这时外部定位的/frame_id和/child_frame_id名称不限定；而后者需要提供时间戳才能正确发送。

mavros/odometry/out:
 
uwb_pos.pose.pose.position.x=pos_3d[0];
uwb_pos.pose.pose.position.y=pos_3d[1];
uwb_pos.pose.pose.position.z=pos_3d[2];
uwb_pos.pose.pose.orientation.x = quaternion[1];
uwb_pos.pose.pose.orientation.y = quaternion[2];
uwb_pos.pose.pose.orientation.z = quaternion[3];
uwb_pos.pose.pose.orientation.w = quaternion[0];
uwb_pos.header.frame_id="odom";
uwb_pos.child_frame_id="base_link";
uwb_pub.publish(uwb_pos);

mavros/vision_pose/pose:
 
vision.pose.position.x = x;
vision.pose.position.y = y;
vision.pose.position.z = z;
vision.pose.orientation.x = q_mocap.x();
vision.pose.orientation.y = q_mocap.y();
vision.pose.orientation.z = q_mocap.z();
vision.pose.orientation.w = q_mocap.w();
vision.header.stamp = ros::Time::now();
vision_pub.publish(vision);

1 外部定位

往往外部定位装置参考坐标系和px4系统参考坐标系不会完全一致，这时需要使用一个转换。这里通过ROS(Robot Operating System)端进行转换，需要注意的是ROS中和PX4的local和world坐标系是不一样的。

See REP105: Coordinate Frames for Mobile Platforms (opens new window)for more information about ROS frames.

ROS中使用ENU坐标系作为协议，所以外部定位数据应该遵守ENU坐标系发送，而MAVROS负责转换为NED坐标系下供PX4使用。(FRD对应FLU坐标系)

当外部定位机体坐标系和px4本地坐标系名称不一致时，使用MAVROS中的odometry插件可以较容易的处理。通过TF静态变化可以提供转换。详情查看：

mavros_extras: mavros::extra_plugins::OdometryPlugin Class Referencehttps://docs.ros.org/en/noetic/api/mavros_extras/html/classmavros_1_1extra__plugins_1_1OdometryPlugin.html#details通过添加以下代码到ROS的launch文件中：

<node pkg="tf" type="static_transform_publisher" name="tf_baseLink_externalPoseChildFrame"
        args="0 0 0 <yaw> <pitch> <roll> base_link <external_pose_child_frame> 1000"/>
<node pkg="tf" type="static_transform_publisher" name="tf_odom_externalPoseParentFrame"
        args="0 0 0 <yaw> <pitch> <roll> odom <external_pose_parent_frame> 1000"/>

确定你的外部定位装置tf转换是正确的，这里可以参考tf - ROS Wikihttps://wiki.ros.org/tf#static_transform_publisher

注意：当使用上面MAVROS插件的方式进行转换，需要确保外部定位的frame_id和child_frame_id不存在其他的TF变换，否则可能会破坏上述变换。

需要注意的是，上面launch文件中写入的base_link和odom会随着      mavros/launch/px4_config.yaml 更新而改变，在文件中找到odometry frames那一栏查看就行（？？？？？？？？？？？？？）

2 Reference

Using Vision or Motion Capture Systems for Position Estimation | PX4 User Guide (v1.14)

Using Vision or Motion Capture Systems for Position Estimation | PX4 User Guide (v1.14)

REP 105 -- Coordinate Frames for Mobile Platforms (ROS.org)

mavros_extras: mavros::extra_plugins::OdometryPlugin Class Reference