Added external pose input message and pose relay script.

python/fusion_engine_client/analysis/attitude.py

@@ -49,3 +49,133 @@ def get_enu_rotation_matrix(latitude, longitude, deg=False):
     C_enu_e = C_ned_e[(1, 0, 2), :]
     C_enu_e[2, :] = -C_enu_e[2, :]
     return C_enu_e
+
+
+def axis_rotation_dcm(angle, axis, deg=False):
+    """!
+    @brief Create a direction cosine matrix rotating a coordinate frame around the specified axis.
+
+    The resulting DCM represents a passive transformation of a vector expressed in the original coordinate frame (@f$
+    \alpha @f$) to one expressed in the rotated coordinate frame (@f$ \beta @f$).
+
+    @param angle The desired rotation angle.
+    @param axis The axis to be rotated around (@c 'x', @c 'y', or @c 'z').
+    @param deg If @c True, interpret the input angles as degrees. Otherwise, interpret as radians.
+
+    @return The 3x3 rotation matrix @f$ C_\alpha^\beta @f$.
+    """
+    if deg:
+        angle = np.deg2rad(angle)
+
+    cos = np.cos(angle)
+    sin = np.sin(angle)
+
+    if axis == 'x':
+        return np.array(((1,    0,   0),
+                         (0,  cos, sin),
+                         (0, -sin, cos)))
+    elif axis == 'y':
+        return np.array(((cos,  0, -sin),
+                         (  0,  1,    0),
+                         (sin,  0,  cos)))
+    elif axis == 'z':
+        return np.array((( cos,  sin, 0),
+                         (-sin,  cos, 0),
+                         (   0,    0, 1)))
+    else:
+        raise RuntimeError('Invalid axis specification.')
+
+
+def euler_angles_to_dcm(x, y=None, z=None, order='321', deg=False):
+    """!
+    @brief Convert an Euler (Tait-Bryan) angle attitude (in radians) into a direction cosine matrix.
+
+    The input angles represent the rotation from coordinate frame @f$ \alpha @f$ to coordinate frame @f$ \beta @f$ (@f$
+    C_\alpha^\beta @f$. This is frequently the rotation from the navigation frame (local level frame) to the body frame,
+    @f$ C_n^b @f$.
+
+    @note
+    This DCM represents a passive transformation of a vector from one coordinate frame to another, as opposed to an
+    active rotation of a vector within a single coordinate frame.
+
+    This function supports any intrinsic rotation order through the @c order argument. The most common convention is a
+    three-two-one rotation (@c '321'), in which the coordinate frame is first rotated around the Z-axis (i.e., the third
+    axis) by the angle @c z, then the second axis in the resulting intermediate coordinate frame (the Y'-axis), and
+    finally the third axis in the last intermediate coordinate frame (the X''-axis). 3-2-1 rotation angles are commonly
+    referred to as yaw, pitch, and roll for the Z, Y, and X rotations respectively.
+
+    You may specify all three angles separately using the @c x, @c y, and @c z parameters, or provide a Numpy array
+    containing all three angles.
+
+    @param x The angle to rotate around the X-axis (i.e., axis 1). May be a Numpy array containing the rotation angles
+           for all three axes in the order X, Y, Z.
+    @param y The angle to rotate around the Y-axis (i.e., axis 2).
+    @param z The angle to rotate around the Z-axis (i.e., axis 3).
+    @param order A string specifying the rotation order to be applied.
+    @param deg If @c True, interpret the input angles as degrees. Otherwise, interpret as radians.
+
+    @return The 3x3 rotation matrix rotating from the @f$ \alpha @f$ frame to the @f$ \beta @f$ frame (@f$
+            C_\alpha^\beta @f$).
+    """
+    if isinstance(x, np.ndarray):
+        if y is not None or z is not None:
+            raise RuntimeError('Cannot specify Y/Z angles when using a Numpy array.')
+        else:
+            angles = x
+    else:
+        if y is None or z is None:
+            raise RuntimeError('You must specify Y and Z angles.')
+        else:
+            angles = np.array((x, y, z))
+
+    if deg:
+        angles = np.deg2rad(angles)
+
+    # 3-2-1 order written explicitly for efficiency since it's the most common order. Taken from Groves section 2.2.2.
+    if order == '321':
+        sin_phi, sin_theta, sin_psi = np.sin(angles)
+        cos_phi, cos_theta, cos_psi = np.cos(angles)
+
+        return np.array((
+            (cos_theta * cos_psi, cos_theta * sin_psi, -sin_theta),
+            (-cos_phi * sin_psi + sin_phi * sin_theta * cos_psi,
+             cos_phi * cos_psi + sin_phi * sin_theta * sin_psi,
+             sin_phi * cos_theta),
+            (sin_phi * sin_psi + cos_phi * sin_theta * cos_psi,
+             -sin_phi * cos_psi + cos_phi * sin_theta * sin_psi,
+             cos_phi * cos_theta)
+        ))
+    else:
+        dcm = {}
+        dcm['1'] = axis_rotation_dcm(angles[0], axis='x')
+        dcm['2'] = axis_rotation_dcm(angles[1], axis='y')
+        dcm['3'] = axis_rotation_dcm(angles[2], axis='z')
+
+        return dcm[order[2]].dot(dcm[order[1]]).dot(dcm[order[0]])
+
+
+def dcm_to_euler_angles(dcm, order='321', deg=False):
+    """!
+    @brief Convert a direction cosine matrix into an Euler (Tait-Bryan) angle attitude representation.
+
+    Convert a DCM representing the rotation from coordinate frame @f$ \alpha @f$ to coordinate frame @f$ \beta @f$ into
+    corresponding Euler angles with the specified rotation order.
+
+    @param dcm The 3x3 rotation matrix.
+    @param order The desired Euler angle order.
+    @param deg If @c True, return angles in degrees. Otherwise, return radians.
+
+    @return A Numpy array containing the X, Y, and Z rotation angles.
+    """
+    if order == '321':
+        x = np.arctan2(dcm[1, 2], dcm[2, 2])
+        y = -np.arcsin(dcm[0, 2])
+        z = np.arctan2(dcm[0, 1], dcm[0, 0])
+    else:
+        raise RuntimeError('Unsupported rotation order.')
+
+    result = np.array((x, y, z))
+    if deg:
+        result = np.rad2deg(result)
+
+    return result

python/fusion_engine_client/messages/defs.py

@@ -106,6 +106,7 @@ class MessageType(IntEnum):
     IMU_INPUT = 11004
     GNSS_ATTITUDE_OUTPUT = 11005
     RAW_GNSS_ATTITUDE_OUTPUT = 11006
+    EXTERNAL_POSE_INPUT = 11007
 
     # Vehicle measurement messages.
     DEPRECATED_WHEEL_SPEED_MEASUREMENT = 11101

python/fusion_engine_client/messages/measurements.py

@@ -1417,6 +1417,130 @@ def to_numpy(cls, messages: Sequence['RawGNSSAttitudeOutput']):
         result.update(MeasurementDetails.to_numpy([m.details for m in messages]))
         return result
 
+################################################################################
+# External Pose Measurements
+################################################################################
+
+
+class ExternalPoseInput(MessagePayload):
+    """! @brief External pose measurement input.
+
+    Position is expressed in the ECEF frame. Orientation is the yaw, pitch, roll
+    (YPR) angles in the local ENU frame. Velocity is expressed in the local ENU
+    frame. Any elements that are not available should be set to `nan`. Standard
+    deviation fields are specified in the same units.
+    """
+    MESSAGE_TYPE = MessageType.EXTERNAL_POSE_INPUT
+    MESSAGE_VERSION = 0
+
+    FLAG_RESET_POSITION_DATA = 0x1
+
+    _STRUCT = struct.Struct('<B3xI3d3f3f3f3f3f')
+
+    def __init__(self):
+        self.details = MeasurementDetails()
+
+        self.solution_type = SolutionType.Invalid
+
+        self.flags = 0
+
+        self.position_ecef_m = np.full((3,), np.nan)
+        self.position_std_ecef_m = np.full((3,), np.nan)
+
+        self.ypr_deg = np.full((3,), np.nan)
+        self.ypr_std_deg = np.full((3,), np.nan)
+
+        self.velocity_enu_mps = np.full((3,), np.nan)
+        self.velocity_std_enu_mps = np.full((3,), np.nan)
+
+    def pack(self, buffer: bytes = None, offset: int = 0,
+             return_buffer: bool = True) -> (bytes, int):
+        if buffer is None:
+            buffer = bytearray(self.calcsize())
+            offset = 0
+
+        initial_offset = offset
+
+        offset += self.details.pack(buffer, offset, return_buffer=False)
+
+        offset += self.pack_values(
+            self._STRUCT, buffer, offset,
+            int(self.solution_type),
+            self.flags,
+            self.position_ecef_m[0], self.position_ecef_m[1], self.position_ecef_m[2],
+            self.position_std_ecef_m[0], self.position_std_ecef_m[1], self.position_std_ecef_m[2],
+            self.ypr_deg[0], self.ypr_deg[1], self.ypr_deg[2],
+            self.ypr_std_deg[0], self.ypr_std_deg[1], self.ypr_std_deg[2],
+            self.velocity_enu_mps[0], self.velocity_enu_mps[1], self.velocity_enu_mps[2],
+            self.velocity_std_enu_mps[0], self.velocity_std_enu_mps[1], self.velocity_std_enu_mps[2])
+        if return_buffer:
+            return buffer
+        else:
+            return offset - initial_offset
+
+    def unpack(self, buffer: bytes, offset: int = 0,
+               message_version: int = MessagePayload._UNSPECIFIED_VERSION) -> int:
+        initial_offset = offset
+
+        offset += self.details.unpack(buffer, offset)
+
+        (solution_type_int,
+         self.flags,
+         self.position_ecef_m[0], self.position_ecef_m[1], self.position_ecef_m[2],
+         self.position_std_ecef_m[0], self.position_std_ecef_m[1], self.position_std_ecef_m[2],
+         self.ypr_deg[0], self.ypr_deg[1], self.ypr_deg[2],
+         self.ypr_std_deg[0], self.ypr_std_deg[1], self.ypr_std_deg[2],
+         self.velocity_enu_mps[0], self.velocity_enu_mps[1], self.velocity_enu_mps[2],
+         self.velocity_std_enu_mps[0], self.velocity_std_enu_mps[1], self.velocity_std_enu_mps[2]) = \
+            self._STRUCT.unpack_from(buffer, offset)
+        offset += self._STRUCT.size
+
+        self.solution_type = SolutionType(solution_type_int)
+
+        return offset - initial_offset
+
+    @classmethod
+    def calcsize(cls) -> int:
+        return MeasurementDetails.calcsize() + cls._STRUCT.size
+
+    @classmethod
+    def to_numpy(cls, messages: Sequence['ExternalPoseInput']):
+        result = {
+            'solution_type': np.array([int(m.solution_type) for m in messages], dtype=int),
+            'flags': np.array([m.flags for m in messages], dtype=np.uint32),
+            'position_ecef_m': np.array([m.position_ecef_m for m in messages]).T,
+            'position_std_ecef_m': np.array([m.position_std_ecef_m for m in messages]).T,
+            'ypr_deg': np.array([m.ypr_deg for m in messages]).T,
+            'ypr_std_deg': np.array([m.ypr_std_deg for m in messages]).T,
+            'velocity_enu_mps': np.array([m.velocity_enu_mps for m in messages]).T,
+            'velocity_std_enu_mps': np.array([m.velocity_std_enu_mps for m in messages]).T,
+        }
+        result.update(MeasurementDetails.to_numpy([m.details for m in messages]))
+        return result
+
+    def __getattr__(self, item):
+        if item == 'p1_time':
+            return self.details.p1_time
+        else:
+            return super().__getattr__(item)
+
+    def __repr__(self):
+        result = super().__repr__()[:-1]
+        result += f', solution_type={self.solution_type}, position_ecef={self.position_ecef_m}, ' \
+                  f'ypr_deg={self.ypr_deg}, velocity_enu_mps={self.velocity_enu_mps}]'
+        return result
+
+    def __str__(self):
+        string = 'External Pose Measurement @ %s\n' % str(self.details.p1_time)
+        string += '  Solution type: %s\n' % self.solution_type.name
+        string += '  Position (ECEF): %.2f, %.2f, %.2f (m, m, m)\n' % tuple(self.position_ecef_m)
+        string += '  Position std (ECEF): %.2f, %.2f, %.2f (m, m, m)\n' % tuple(self.position_std_ecef_m)
+        string += '  YPR: %.2f, %.2f, %.2f (deg, deg, deg)\n' % tuple(self.ypr_deg)
+        string += '  YPR std: %.2f, %.2f, %.2f (deg, deg, deg)\n' % tuple(self.ypr_std_deg)
+        string += '  Velocity (ENU): %.2f, %.2f, %.2f (m/s, m/s, m/s)\n' % tuple(self.velocity_enu_mps)
+        string += '  Velocity std (ENU): %.2f, %.2f, %.2f (m/s, m/s, m/s)' % tuple(self.velocity_std_enu_mps)
+        return string
+
 ################################################################################
 # Binary Sensor Data Definitions
 ################################################################################

python/tests/test_message_defs.py

@@ -98,7 +98,7 @@ def test_find_message_types():
     # Use wildcards to match multiple types.
     assert MessagePayload.find_matching_message_types(['pose*']) == {MessageType.POSE, MessageType.POSE_AUX}
     assert MessagePayload.find_matching_message_types(['*pose*']) == {MessageType.POSE, MessageType.POSE_AUX,
-                                                                      MessageType.ROS_POSE}
+                                                                      MessageType.EXTERNAL_POSE_INPUT, MessageType.ROS_POSE}
     assert MessagePayload.find_matching_message_types(['pose*', 'gnssi*']) == {MessageType.POSE, MessageType.POSE_AUX,
                                                                                MessageType.GNSS_INFO}
     assert MessagePayload.find_matching_message_types(['pose*,gnssi*']) == {MessageType.POSE, MessageType.POSE_AUX,

src/point_one/fusion_engine/messages/defs.h

@@ -54,6 +54,7 @@ enum class MessageType : uint16_t {
   IMU_INPUT = 11004, ///< @ref IMUInput
   GNSS_ATTITUDE_OUTPUT = 11005, ///< @ref GNSSAttitudeOutput
   RAW_GNSS_ATTITUDE_OUTPUT = 11006, ///< @ref RawGNSSAttitudeOutput
+  EXTERNAL_POSE_INPUT = 11007, ///< @ref ExternalPoseInput
 
   // Vehicle measurement messages.
   DEPRECATED_WHEEL_SPEED_MEASUREMENT =
@@ -176,6 +177,9 @@ P1_CONSTEXPR_FUNC const char* to_string(MessageType type) {
     case MessageType::RAW_GNSS_ATTITUDE_OUTPUT:
       return "Raw GNSS Attitude Output";
 
+    case MessageType::EXTERNAL_POSE_INPUT:
+      return "External Pose Input";
+
     case MessageType::DEPRECATED_WHEEL_SPEED_MEASUREMENT:
       return "Wheel Speed Measurement";
 
@@ -353,6 +357,7 @@ P1_CONSTEXPR_FUNC bool IsCommand(MessageType message_type) {
     case MessageType::IMU_INPUT:
     case MessageType::GNSS_ATTITUDE_OUTPUT:
     case MessageType::RAW_GNSS_ATTITUDE_OUTPUT:
+    case MessageType::EXTERNAL_POSE_INPUT:
     case MessageType::DEPRECATED_WHEEL_SPEED_MEASUREMENT:
     case MessageType::DEPRECATED_VEHICLE_SPEED_MEASUREMENT:
     case MessageType::WHEEL_TICK_INPUT:

src/point_one/fusion_engine/messages/measurements.h

@@ -1264,6 +1264,87 @@ struct P1_ALIGNAS(4) RawGNSSAttitudeOutput : public MessagePayload {
   float position_std_enu_m[3] = {NAN, NAN, NAN};
 };
 
+////////////////////////////////////////////////////////////////////////////////
+// External Pose Measurements
+////////////////////////////////////////////////////////////////////////////////
+
+/**
+ * @brief External pose measurement input (@ref
+ *        MessageType::EXTERNAL_POSE_INPUT, version 1.0).
+ * @ingroup measurement_messages
+ *
+ * This message is an input to the device containing pose and velocity
+ * measurements generated by an external source (for example, another Point One
+ * device or a vision system).
+ *
+ * Position is expressed in the ECEF frame, and velocity is expressed in the
+ * local ENU frame. @ref position_ecef_m should correspond to the output lever
+ * arm point configured on the receiving device (see @ref
+ * ConfigType::OUTPUT_LEVER_ARM), so that the position matches the point the
+ * device will report in its own @ref PoseMessage after initialization.
+ *
+ * Orientation is specified as yaw, pitch, roll (YPR) angles in the local ENU
+ * frame, following the same intrinsic Euler-321 convention as @ref
+ * PoseMessage::ypr_deg.
+ *
+ * Any elements that are not available should be set to `NAN`. Standard
+ * deviation fields are specified in the same units as the corresponding
+ * measurement.
+ */
+struct P1_ALIGNAS(4) ExternalPoseInput : public MessagePayload {
+  static constexpr MessageType MESSAGE_TYPE = MessageType::EXTERNAL_POSE_INPUT;
+  static constexpr uint8_t MESSAGE_VERSION = 0;
+
+  /** Equivalent to sending a reset message. See @ref ResetRequest. */
+  static constexpr uint32_t FLAG_RESET_POSITION_DATA = 0x01;
+
+  /** Measurement timestamp and additional information. */
+  MeasurementDetails details;
+
+  /** The pose solution type provided by the external source. */
+  SolutionType solution_type = SolutionType::Invalid;
+
+  uint8_t reserved[3] = {0};
+
+  /** A bitmask of flags associated with the measurement. */
+  uint32_t flags = 0;
+
+  /**
+   * An estimate of the device's output position (in meters), resolved in the
+   * ECEF frame.
+   */
+  double position_ecef_m[3] = {NAN, NAN, NAN};
+
+  /**
+   * An estimate of the device's output position standard deviation (in meters),
+   * resolved in the ECEF frame.
+   */
+  float position_std_ecef_m[3] = {NAN, NAN, NAN};
+
+  /** An estimate of the device's output orientation (in degrees), resolved in
+   * the local ENU tangent plane. See @ref PoseMessage::ypr_deg for a complete
+   * rotation definition.
+   */
+  float ypr_deg[3] = {NAN, NAN, NAN};
+
+  /** An estimate of the device's output orientation standard deviation (in
+   * degrees).
+   */
+  float ypr_std_deg[3] = {NAN, NAN, NAN};
+
+  /**
+   * An estimate of the device's output velocity (in m/s), resolved in the local
+   * ENU tangent plane.
+   */
+  float velocity_enu_mps[3] = {NAN, NAN, NAN};
+
+  /**
+   * An estimate of the device's output velocity standard deviation (in m/s),
+   * resolved in the local ENU tangent plane.
+   */
+  float velocity_std_enu_mps[3] = {NAN, NAN, NAN};
+};
+
 ////////////////////////////////////////////////////////////////////////////////
 // Binary Sensor Data Definitions
 ////////////////////////////////////////////////////////////////////////////////