Skip to content

Create generalized proxy tool to forward messages #448

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
bhers4 merged 3 commits into from
Oct 29, 2026
Merged

Create generalized proxy tool to forward messages #448

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
ad13739
Added a multi-client TCP server output transport.
bhers4 Oct 29, 2026
f1341c6
Moved the pose relay helpers into utils/pose_relay.py.
bhers4 Oct 29, 2026
8959119
Added the p1_proxy tool to forward measurements between devices.
bhers4 Oct 29, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 7 additions & 0 deletions python/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,13 @@ FusionEngine message specification.
- [p1_lband_extract](fusion_engine_client/applications/p1_lband_extract.py) - Extract L-band data bits contained from a
log of FusionEngine `LBandFrameMessage` messages
- [p1_print](fusion_engine_client/applications/p1_print.py) - Legacy alias for `p1_capture`.
- [p1_proxy](fusion_engine_client/applications/p1_proxy.py) - Forward raw wheel speed, vehicle speed, IMU, and pose
output from a source device to one or more target devices as the corresponding input messages.
- Pose is forwarded as `ExternalPoseInput`, optionally transformed to account for lever arms and relative device
mounting.
- A target of the form `tcp://:<port>` listens for incoming connections instead of connecting out. Any number of
clients may connect and disconnect at any time, and all of them receive the forwarded messages.
- A target that is unreachable or disconnects does not stop the proxy; it is retried automatically.

### Directory Structure
- `python/` - Python source files
Expand Down
179 changes: 3 additions & 176 deletions python/examples/pose_relay.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,197 +11,24 @@
import time

import numpy as np
import pymap3d as pm

# Add the Python root directory (fusion-engine-client/python/) to the import
# search path to enable FusionEngine imports if this application is being run
# directly out of the repository and is not installed as a pip package.
root_dir = os.path.normpath(os.path.join(os.path.dirname(__file__), '..'))
sys.path.insert(0, root_dir)

from fusion_engine_client.analysis.attitude import (
get_enu_rotation_matrix, euler_angles_to_dcm, dcm_to_euler_angles,
)
from fusion_engine_client.messages import MessageType, PoseMessage
from fusion_engine_client.messages.measurements import (
ExternalPoseInput, SystemTimeSource,
)
from fusion_engine_client.messages.defs import SolutionType, SourceIdentifier, Timestamp
from fusion_engine_client.messages.defs import SolutionType, SourceIdentifier
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
from fusion_engine_client.utils.transport_utils import *
from fusion_engine_client.utils.bin_utils import bytes_to_hex
from fusion_engine_client.utils.pose_relay import transform_pose_to_target, transformed_pose_to_external_pose_input
from fusion_engine_client.utils.transport_utils import *

logger = logging.getLogger('point_one.fusion_engine.pose_relay')

# Coordinate frame abbreviations:
# enu - Local navigation frame (East, North, Up)
# ecef - Earth-Centered, Earth-Fixed frame
# sb - Source body frame
# tb - Target body frame

# Calculates the target YPR in the local navigation frame given the source YPR
# in the local navigation frame and the target-to-source-body rotation.
def calculate_ypr_enu_to_tb_deg(ypr_enu_to_sb_deg,
rotmat_tb_to_sb) -> np.ndarray:
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_tb_to_enu = rotmat_sb_to_enu @ rotmat_tb_to_sb
rpy = dcm_to_euler_angles(rotmat_tb_to_enu.T, order='321', deg=True)
return rpy[::-1]


# Calculates the Jacobian of the target YPR with respect to the source YPR,
# given the target-to-source-body rotation.
def calculate_jacobian_ypr_target_wrt_source(ypr_enu_to_sb_deg,
rotmat_tb_to_sb,
delta_deg=1e-6) -> np.ndarray:
# Numerical approximation of the Jacobian of the target YPR with respect to
# the source YPR, using central differences.
jacobian = np.zeros((3, 3))
for j in range(3):
ypr_enu_to_sb_deg_plus = ypr_enu_to_sb_deg.copy()
ypr_enu_to_sb_deg_minus = ypr_enu_to_sb_deg.copy()
ypr_enu_to_sb_deg_plus[j] += delta_deg
ypr_enu_to_sb_deg_minus[j] -= delta_deg
ypr_enu_to_tb_deg_plus = \
calculate_ypr_enu_to_tb_deg(ypr_enu_to_sb_deg_plus, rotmat_tb_to_sb)
ypr_enu_to_tb_deg_minus = \
calculate_ypr_enu_to_tb_deg(ypr_enu_to_sb_deg_minus, rotmat_tb_to_sb)
jacobian[:, j] = \
(ypr_enu_to_tb_deg_plus - ypr_enu_to_tb_deg_minus) / (2.0 * delta_deg)
return jacobian


# Helper function to transform source pose to target pose.
def transform_pose_to_target(source_pose_msg,
source_lever_arm_m, target_lever_arm_m,
ypr_tb_to_sb_deg, vec_tb_to_sb_m) -> dict:
have_target_rot = not np.allclose(ypr_tb_to_sb_deg, 0.0)
if have_target_rot:
rotmat_tb_to_sb = \
euler_angles_to_dcm(ypr_tb_to_sb_deg[::-1], order='321', deg=True)
else:
rotmat_tb_to_sb = np.eye(3)

# ---------- Position ----------
source_position_ecef_m = np.array(
pm.geodetic2ecef(source_pose_msg.lla_deg[0],
source_pose_msg.lla_deg[1],
source_pose_msg.lla_deg[2]))

rotmat_ecef_to_enu = get_enu_rotation_matrix(
source_pose_msg.lla_deg[0], source_pose_msg.lla_deg[1], deg=True)
rotmat_enu_to_ecef = rotmat_ecef_to_enu.T

# 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

vec_tb_to_sb_in_ecef_m = rotmat_sb_to_ecef @ vec_tb_to_sb_m

# Apply lever arm correction to get target output position in
# ECEF.
# target_position_ecef_m =
# source_position_ecef_m - (source lever arm in ECEF)
# + vec_tb_to_sb_in_ecef_m + (target lever arm in ECEF)
target_position_ecef_m = source_position_ecef_m \
- rotmat_sb_to_ecef @ source_lever_arm_m + vec_tb_to_sb_in_ecef_m \
+ rotmat_sb_to_ecef @ rotmat_tb_to_sb @ target_lever_arm_m

source_position_enu_cov_m2 = np.diag(source_pose_msg.position_std_enu_m ** 2)
target_position_ecef_cov_m2 = \
rotmat_enu_to_ecef @ source_position_enu_cov_m2 @ rotmat_ecef_to_enu
target_position_ecef_std_m = np.sqrt(np.diag(target_position_ecef_cov_m2))

# ---------- Attitude ----------
if have_target_rot and not np.any(np.isnan(ypr_enu_to_sb_deg)):
# YPR does not transform linearly, so we need to
# pre and post multiply by the Jacobian.
ypr_enu_to_tb_deg = calculate_ypr_enu_to_tb_deg(
ypr_enu_to_sb_deg, rotmat_tb_to_sb)
jacobian = calculate_jacobian_ypr_target_wrt_source(
ypr_enu_to_sb_deg, rotmat_tb_to_sb)
ypr_enu_to_sb_cov_deg2 = np.diag(source_pose_msg.ypr_std_deg ** 2)
ypr_enu_to_tb_cov_deg2 = jacobian @ ypr_enu_to_sb_cov_deg2 @ jacobian.T
ypr_enu_to_tb_std_deg = np.sqrt(np.diag(ypr_enu_to_tb_cov_deg2))
else:
ypr_enu_to_tb_deg = ypr_enu_to_sb_deg.copy()
ypr_enu_to_tb_std_deg = source_pose_msg.ypr_std_deg.copy()

# ---------- Velocity ----------
# IMPORTANT!: Assume target is not moving relative to source,
# and that the source angular rate is zero, whether or not
# the source velocity is non-zero. We print a warning here
# just as a reminder.

have_source_vel = not np.any(np.isnan(source_pose_msg.velocity_body_mps))
if have_source_vel:
velocity_sb_mps = source_pose_msg.velocity_body_mps
# source_velocity_enu_mps = (source body velocity in ENU)
# + (source velocity relative to source body in ENU)
# Last term is assumed zero.
source_velocity_enu_mps = rotmat_sb_to_enu @ velocity_sb_mps

# target_velocity_enu_mps = source_velocity_enu_mps
# + (target velocity wrt source in ENU)
# Last term is assumed zero.
target_velocity_enu_mps = source_velocity_enu_mps
velocity_sb_cov_m2ps2 = np.diag(source_pose_msg.velocity_std_body_mps ** 2)
target_velocity_enu_cov_m2ps2 = \
rotmat_sb_to_enu @ velocity_sb_cov_m2ps2 @ rotmat_enu_to_sb
target_velocity_enu_std_mps = \
np.sqrt(np.diag(target_velocity_enu_cov_m2ps2))
else:
target_velocity_enu_mps = np.full(3, np.nan)
target_velocity_enu_std_mps = np.full(3, np.nan)

return {
"position_ecef_m": target_position_ecef_m,
"position_std_ecef_m": target_position_ecef_std_m,
"ypr_deg": ypr_enu_to_tb_deg,
"ypr_std_deg": ypr_enu_to_tb_std_deg,
"velocity_enu_mps": target_velocity_enu_mps,
"velocity_std_enu_mps": target_velocity_enu_std_mps,
}


# Helper function to convert target pose to ExternalPoseInput message.
def transformed_pose_to_external_pose_input(source_pose_msg, transformed_pose) \
-> ExternalPoseInput:
ext_pose = ExternalPoseInput()

# 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

ext_pose.solution_type = source_pose_msg.solution_type
ext_pose.flags = ExternalPoseInput.FLAG_RESET_POSITION_DATA

ext_pose.position_ecef_m = transformed_pose["position_ecef_m"].astype(np.float64)
ext_pose.position_std_ecef_m = transformed_pose["position_std_ecef_m"].astype(np.float32)

ext_pose.ypr_deg = transformed_pose["ypr_deg"].astype(np.float32)
ext_pose.ypr_std_deg = transformed_pose["ypr_std_deg"].astype(np.float32)

ext_pose.velocity_enu_mps = transformed_pose["velocity_enu_mps"].astype(np.float32)
ext_pose.velocity_std_enu_mps = transformed_pose["velocity_std_enu_mps"].astype(np.float32)

return ext_pose


def main():
parser = ArgumentParser(description="""Relay pose from a source device to
Expand Down
Loading
Loading