From 9801687d367759a7a8a3a110af6e58d0fa46c2e6 Mon Sep 17 00:00:00 2001 From: Karen Leineweber Date: Tue, 7 Jul 2026 13:43:24 -0700 Subject: [PATCH 1/3] Rename visual->external in solution types, change definition for external pose frame in docs --- .../fusion_engine_client/analysis/analyzer.py | 2 +- python/fusion_engine_client/messages/defs.py | 4 +-- src/point_one/fusion_engine/messages/defs.h | 8 +++--- .../fusion_engine/messages/measurements.h | 27 ++++++++++++------- 4 files changed, 24 insertions(+), 17 deletions(-) diff --git a/python/fusion_engine_client/analysis/analyzer.py b/python/fusion_engine_client/analysis/analyzer.py index 7f010b20..aa47b775 100755 --- a/python/fusion_engine_client/analysis/analyzer.py +++ b/python/fusion_engine_client/analysis/analyzer.py @@ -47,7 +47,7 @@ SolutionType.RTKFloat: SolutionTypeInfo(name='RTK Float', style={'color': 'green'}), SolutionType.RTKFixed: SolutionTypeInfo(name='RTK Fixed', style={'color': 'orange'}), SolutionType.PPP: SolutionTypeInfo(name='PPP', style={'color': 'pink'}), - SolutionType.Visual: SolutionTypeInfo(name='Vision', style={'color': 'purple'}), + SolutionType.External: SolutionTypeInfo(name='External', style={'color': 'purple'}), } diff --git a/python/fusion_engine_client/messages/defs.py b/python/fusion_engine_client/messages/defs.py index 84ea71a9..bc5c8abd 100644 --- a/python/fusion_engine_client/messages/defs.py +++ b/python/fusion_engine_client/messages/defs.py @@ -47,8 +47,8 @@ class SolutionType(IntEnum): RTKFloat = 5 # Integrated position using dead reckoning. Integrate = 6 - # Using vision measurements. - Visual = 9 + # Using external (vision, lidar) measurements. + External = 9 # GNSS precise point positioning (PPP) pseudorange/carrier phase solution. PPP = 10 diff --git a/src/point_one/fusion_engine/messages/defs.h b/src/point_one/fusion_engine/messages/defs.h index 229fa146..d400a5b9 100644 --- a/src/point_one/fusion_engine/messages/defs.h +++ b/src/point_one/fusion_engine/messages/defs.h @@ -530,8 +530,8 @@ enum class SolutionType : uint8_t { RTKFloat = 5, /** Integrated position using dead reckoning. */ Integrate = 6, - /** Using vision measurements. */ - Visual = 9, + /** Using external (vision, lidar) measurements. */ + External = 9, /** * GNSS precise point positioning (PPP) pseudorange/carrier phase solution. */ @@ -567,8 +567,8 @@ P1_CONSTEXPR_FUNC const char* to_string(SolutionType type) { case SolutionType::Integrate: return "Dead Reckoning"; - case SolutionType::Visual: - return "Visual Navigation"; + case SolutionType::External: + return "External Navigation"; case SolutionType::PPP: return "PPP GNSS"; diff --git a/src/point_one/fusion_engine/messages/measurements.h b/src/point_one/fusion_engine/messages/measurements.h index bb00e11b..991f83dd 100644 --- a/src/point_one/fusion_engine/messages/measurements.h +++ b/src/point_one/fusion_engine/messages/measurements.h @@ -1278,14 +1278,15 @@ struct P1_ALIGNAS(4) RawGNSSAttitudeOutput : public MessagePayload { * 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. + * local ENU frame. @ref position_ecef_m should correspond to the reference + * point selected by @ref MessageHeader::source_identifier: either the + * external source's own point (for example, a lidar/camera sensor origin), or + * the receiving device output point when the source_identifier + * output-lever-arm flag is set. * * 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. + * PoseMessage::ypr_deg, but body-fixed to the external source frame. * * Any elements that are not available should be set to `NAN`. Standard * deviation fields are specified in the same units as the corresponding @@ -1310,8 +1311,12 @@ struct P1_ALIGNAS(4) ExternalPoseInput : public MessagePayload { uint32_t flags = 0; /** - * An estimate of the device's output position (in meters), resolved in the - * ECEF frame. + * An estimate of the external source's position (in meters), resolved in the + * ECEF frame. This should correspond to the reference point selected by + * @ref MessageHeader::source_identifier: either the source's own point (e.g. + * the lidar/camera sensor origin), for which the receiving device applies the + * configured lever arm, or the device output point when the source_identifier + * output-lever-arm flag is set. */ double position_ecef_m[3] = {NAN, NAN, NAN}; @@ -1321,9 +1326,11 @@ struct P1_ALIGNAS(4) ExternalPoseInput : public MessagePayload { */ 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. + /** An estimate of the external source's orientation (in degrees), resolved + * in the local ENU tangent plane. This describes the frame selected by + * @ref MessageHeader::source_identifier (e.g. the lidar/camera sensor frame); + * the receiving device applies the configured mounting rotation. See @ref + * PoseMessage::ypr_deg for a complete rotation definition. */ float ypr_deg[3] = {NAN, NAN, NAN}; From 94eeb25e0104398590c26a067085e6c441d8e693 Mon Sep 17 00:00:00 2001 From: Karen Leineweber Date: Tue, 7 Jul 2026 14:18:10 -0700 Subject: [PATCH 2/3] Defined new SourceIdentifier enum. --- python/examples/pose_relay.py | 6 ++- python/fusion_engine_client/messages/defs.py | 6 +-- .../fusion_engine_client/parsers/encoder.py | 5 ++- src/point_one/fusion_engine/messages/defs.h | 43 +++++++++++++++++-- .../fusion_engine/messages/measurements.h | 17 ++++---- 5 files changed, 58 insertions(+), 19 deletions(-) diff --git a/python/examples/pose_relay.py b/python/examples/pose_relay.py index 66b8021d..ebb1d989 100644 --- a/python/examples/pose_relay.py +++ b/python/examples/pose_relay.py @@ -26,7 +26,7 @@ from fusion_engine_client.messages.measurements import ( ExternalPoseInput, SystemTimeSource, ) -from fusion_engine_client.messages.defs import SolutionType, Timestamp +from fusion_engine_client.messages.defs import SolutionType, SourceIdentifier, Timestamp from fusion_engine_client.parsers import FusionEngineDecoder, FusionEngineEncoder from fusion_engine_client.utils import trace as logging from fusion_engine_client.utils.argument_parser import ArgumentParser @@ -391,7 +391,9 @@ def main(): source_pose_msg, transformed_pose) # Encode and send. - encoded_data = encoder.encode_message(ext_pose) + encoded_data = encoder.encode_message( + ext_pose, + source_identifier=SourceIdentifier.OUTPUT_LEVER_ARM) logger.debug(bytes_to_hex(encoded_data, bytes_per_row=16, bytes_per_col=2)) target_transport.send(encoded_data) diff --git a/python/fusion_engine_client/messages/defs.py b/python/fusion_engine_client/messages/defs.py index bc5c8abd..8ab5b942 100644 --- a/python/fusion_engine_client/messages/defs.py +++ b/python/fusion_engine_client/messages/defs.py @@ -239,8 +239,6 @@ class SourceIdentifier(IntEnum): class MessageHeader: - INVALID_SOURCE_ID = 0xFFFFFFFF - SYNC0 = 0x2E # '.' SYNC1 = 0x31 # '1' @@ -259,7 +257,7 @@ def __init__(self, message_type: MessageType = MessageType.INVALID): self.message_version: int = 0 self.message_type: MessageType = message_type self.payload_size_bytes: int = 0 - self.source_identifier: int = MessageHeader.INVALID_SOURCE_ID + self.source_identifier: int = SourceIdentifier.INVALID def get_type_string(self): return MessageType.get_type_string(self.message_type) @@ -321,7 +319,7 @@ def pack(self, buffer: bytes = None, offset: int = 0, payload: bytes = None, ret args = (MessageHeader.SYNC0, MessageHeader.SYNC1, self.reserved, self.crc, self.protocol_version, self.message_version, int(self.message_type), self.sequence_number, self.payload_size_bytes, - self.source_identifier) + int(self.source_identifier)) if buffer is None: buffer = struct.pack(MessageHeader._FORMAT, *args) if payload is not None: diff --git a/python/fusion_engine_client/parsers/encoder.py b/python/fusion_engine_client/parsers/encoder.py index cc8c6b09..b1f43796 100644 --- a/python/fusion_engine_client/parsers/encoder.py +++ b/python/fusion_engine_client/parsers/encoder.py @@ -1,4 +1,4 @@ -from ..messages import MessageHeader, MessagePayload +from ..messages import MessageHeader, MessagePayload, SourceIdentifier class FusionEngineEncoder: @@ -15,7 +15,8 @@ def __init__(self): """ self.sequence_number = 0 - def encode_message(self, message: MessagePayload, source_identifier: int = 0) -> (bytes): + def encode_message(self, message: MessagePayload, + source_identifier: SourceIdentifier = SourceIdentifier.OUTPUT_LEVER_ARM) -> (bytes): """! @brief Serialize a message with valid header and payload. diff --git a/src/point_one/fusion_engine/messages/defs.h b/src/point_one/fusion_engine/messages/defs.h index d400a5b9..6d8a0475 100644 --- a/src/point_one/fusion_engine/messages/defs.h +++ b/src/point_one/fusion_engine/messages/defs.h @@ -585,6 +585,45 @@ inline p1_ostream& operator<<(p1_ostream& stream, SolutionType type) { return stream; } +/** + * @brief Output solution/measurement source identifiers. + */ +enum class SourceIdentifier : uint32_t { + // 0 - 99 is reserved for pose solutions. + /** + * The location on the vehicle defined by the device's output lever arm + * setting. + */ + OUTPUT_LEVER_ARM = 0, + + // 100 - 199 is reserved for IMUs. + + // 300 - 399 is reserved for GNSS receivers/antennas. + /** Primary GNSS antenna. */ + PRIMARY_GNSS_ANTENNA = 300, + /** + * Secondary/auxiliary GNSS antenna (heading + pitch/roll for dual-antenna + * systems). + */ + SECONDARY_GNSS_ANTENNA = 301, + + // 500 - 599 is reserved for external pose sources, such as an external SLAM + // or VIO/LIO. + /** Invalid source identifier. */ + INVALID = 0xFFFFFFFF, +}; + +static constexpr SourceIdentifier INVALID_SOURCE_ID = SourceIdentifier::INVALID; + +/** + * @brief @ref SourceIdentifier stream operator. + * @ingroup enum_definitions + */ +inline p1_ostream& operator<<(p1_ostream& stream, SourceIdentifier id) { + stream << static_cast(id); + return stream; +} + /** @} */ /** @@ -598,8 +637,6 @@ struct P1_ALIGNAS(4) MessageHeader { static constexpr uint8_t SYNC0 = 0x2E; // '.' static constexpr uint8_t SYNC1 = 0x31; // '1' - static constexpr uint32_t INVALID_SOURCE_ID = 0xFFFFFFFF; - /** * The maximum expected message size (in bytes), used for sanity checking. */ @@ -638,7 +675,7 @@ struct P1_ALIGNAS(4) MessageHeader { uint32_t payload_size_bytes = 0; /** Identifies the source of the serialized data. */ - uint32_t source_identifier = INVALID_SOURCE_ID; + SourceIdentifier source_identifier = SourceIdentifier::INVALID; }; /** diff --git a/src/point_one/fusion_engine/messages/measurements.h b/src/point_one/fusion_engine/messages/measurements.h index 991f83dd..a819f7cc 100644 --- a/src/point_one/fusion_engine/messages/measurements.h +++ b/src/point_one/fusion_engine/messages/measurements.h @@ -1279,10 +1279,10 @@ struct P1_ALIGNAS(4) RawGNSSAttitudeOutput : public MessagePayload { * * Position is expressed in the ECEF frame, and velocity is expressed in the * local ENU frame. @ref position_ecef_m should correspond to the reference - * point selected by @ref MessageHeader::source_identifier: either the - * external source's own point (for example, a lidar/camera sensor origin), or - * the receiving device output point when the source_identifier - * output-lever-arm flag is set. + * point selected by @ref MessageHeader::source_identifier - either the external + * source's own point (for example, a lidar/camera sensor origin), or the + * receiving device output point when the source identifier is set to @ref + * SourceIdentifier::OUTPUT_LEVER_ARM. * * Orientation is specified as yaw, pitch, roll (YPR) angles in the local ENU * frame, following the same intrinsic Euler-321 convention as @ref @@ -1313,10 +1313,10 @@ struct P1_ALIGNAS(4) ExternalPoseInput : public MessagePayload { /** * An estimate of the external source's position (in meters), resolved in the * ECEF frame. This should correspond to the reference point selected by - * @ref MessageHeader::source_identifier: either the source's own point (e.g. + * @ref MessageHeader::source_identifier - either the source's own point (e.g. * the lidar/camera sensor origin), for which the receiving device applies the - * configured lever arm, or the device output point when the source_identifier - * output-lever-arm flag is set. + * configured lever arm, or the device output point when the source identifier + * is set to @ref SourceIdentifier::OUTPUT_LEVER_ARM. */ double position_ecef_m[3] = {NAN, NAN, NAN}; @@ -1326,7 +1326,8 @@ struct P1_ALIGNAS(4) ExternalPoseInput : public MessagePayload { */ float position_std_ecef_m[3] = {NAN, NAN, NAN}; - /** An estimate of the external source's orientation (in degrees), resolved + /** + * An estimate of the external source's orientation (in degrees), resolved * in the local ENU tangent plane. This describes the frame selected by * @ref MessageHeader::source_identifier (e.g. the lidar/camera sensor frame); * the receiving device applies the configured mounting rotation. See @ref From 81c6011c94596d9f7e20d0453fa07d525a1eb82d Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Tue, 28 Jul 2026 17:41:32 -0400 Subject: [PATCH 3/3] Keep MessageHeader.INVALID_SOURCE_ID for convenience/compatibility. --- python/fusion_engine_client/messages/defs.py | 2 ++ src/point_one/fusion_engine/messages/defs.h | 5 +++-- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/python/fusion_engine_client/messages/defs.py b/python/fusion_engine_client/messages/defs.py index 8ab5b942..7101846c 100644 --- a/python/fusion_engine_client/messages/defs.py +++ b/python/fusion_engine_client/messages/defs.py @@ -242,6 +242,8 @@ class MessageHeader: SYNC0 = 0x2E # '.' SYNC1 = 0x31 # '1' + INVALID_SOURCE_ID = SourceIdentifier.INVALID + SYNC = bytes((SYNC0, SYNC1)) _FORMAT = '