Added external pose input message and pose relay script.#393
Added external pose input message and pose relay script.#393kcleineweber merged 3 commits intomasterfrom
Conversation
| * measurements generated by an external source (for example, another Point One | ||
| * device or a vision system). | ||
| * | ||
| * Position and velocity are expressed in the ECEF frame. @ref position_ecef_m |
| case MessageType::VEHICLE_TICK_INPUT: | ||
| case MessageType::WHEEL_SPEED_INPUT: | ||
| case MessageType::VEHICLE_SPEED_INPUT: | ||
| case MessageType::EXTERNAL_POSE_INPUT: |
There was a problem hiding this comment.
Keep in the same order as the enum definition please
adamshapiro0
left a comment
adamshapiro0
left a comment
There was a problem hiding this comment.
Looks good, couple minor things
There was a problem hiding this comment.
Pull request overview
This PR introduces a new FusionEngine measurement input (ExternalPoseInput) for supplying externally generated pose/velocity to a device, along with a Python example (pose_relay.py) that relays/transforms pose from a source device to a target device.
Changes:
- Added
ExternalPoseInputmessage definition in both C++ and Python, and registered the newMessageTypevalue. - Added
pose_relay.pyexample script to transform/forward pose between devices using lever arms and mounting offsets. - Added Euler/DCM helper utilities in
analysis/attitude.pyand updated a Python test to include the new message type in wildcard matching.
Reviewed changes
Copilot reviewed 7 out of 7 changed files in this pull request and generated 4 comments.
Show a summary per file
| File | Description |
|---|---|
| src/point_one/fusion_engine/messages/measurements.h | Adds the C++ ExternalPoseInput payload definition and documentation. |
| src/point_one/fusion_engine/messages/defs.h | Registers the new message type, string name, and command classification. |
| python/fusion_engine_client/messages/defs.py | Registers the new Python MessageType enum value. |
| python/fusion_engine_client/messages/measurements.py | Adds Python ExternalPoseInput pack/unpack and numpy conversion support. |
| python/fusion_engine_client/analysis/attitude.py | Adds DCM/Euler conversion helpers used by the relay example. |
| python/examples/pose_relay.py | New example script to receive PoseMessage, transform it, and send ExternalPoseInput. |
| python/tests/test_message_defs.py | Updates wildcard matching expectation to include EXTERNAL_POSE_INPUT. |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
| * Position and velocity are expressed in the ECEF 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. |
There was a problem hiding this comment.
The struct-level doc says “Position and velocity are expressed in the ECEF frame”, but this message’s velocity fields are explicitly ENU (velocity_enu_mps, velocity_std_enu_mps). Please correct the description so it matches the actual field definitions (ECEF position, ENU velocity).
| * Position and velocity are expressed in the ECEF 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. | |
| * Position is expressed in the ECEF frame (see @ref position_ecef_m) and | |
| * velocity is expressed in the local ENU frame (see @ref velocity_enu_mps). | |
| * @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. |
| # We will still pass through NAN attitude, but for the sake of | ||
| # the position transformation we can treat it as no rotation. | ||
| ypr_enu_to_sb_deg = source_pose_msg.ypr_deg | ||
| if np.any(np.isnan(ypr_enu_to_sb_deg)): | ||
| rotmat_enu_to_sb = np.eye(3) | ||
| else: | ||
| rotmat_enu_to_sb = euler_angles_to_dcm( | ||
| ypr_enu_to_sb_deg[::-1], order='321', deg=True) | ||
| rotmat_sb_to_enu = rotmat_enu_to_sb.T | ||
| rotmat_sb_to_ecef = rotmat_enu_to_ecef @ rotmat_sb_to_enu | ||
|
|
There was a problem hiding this comment.
When the source YPR contains NaNs, the code treats the ENU->source-body rotation as identity for position lever-arm corrections. If either lever arm/translation/target rotation is non-zero, this produces an incorrect target ECEF position (and later velocity) rather than “no rotation”. Consider skipping forwarding until attitude is valid (or only allowing zero lever arms/transforms when attitude is NaN), and emit a clear warning to the user.
| # Timestamps. | ||
| if source_pose_msg.gps_time: | ||
| ext_pose.details.measurement_time = source_pose_msg.gps_time | ||
| ext_pose.details.measurement_time_source = SystemTimeSource.GPS_TIME | ||
| else: | ||
| ext_pose.details.measurement_time = Timestamp() | ||
| ext_pose.details.measurement_time_source = SystemTimeSource.INVALID | ||
|
|
There was a problem hiding this comment.
If GPS time isn’t available, the script currently sends measurement_time_source=INVALID with an invalid timestamp. Since PoseMessage always includes p1_time, it would be more accurate to set measurement_time=source_pose_msg.p1_time and measurement_time_source=SystemTimeSource.P1_TIME so the target can preserve the source measurement time base instead of using arrival time.
| source_transport.close() | ||
| target_transport.close() | ||
|
|
||
| except KeyboardInterrupt: | ||
| pass | ||
| finally: | ||
| elapsed = (datetime.now() - start_time).total_seconds() | ||
| logger.info(f"Done. Pose relayed in {elapsed:.1f} seconds.") |
There was a problem hiding this comment.
source_transport.close() / target_transport.close() are only called on the normal (non-interrupted) path. On KeyboardInterrupt (or any other exception), the transports may remain open until process exit. Consider moving the close calls into the finally block (guarded by if transport is not None) to ensure cleanup.
New Features
ExternalPoseInputmessage definition.pose_relay.pyexample script for transmitting pose from a source device to a target device.