From ad137394429b930ab48832dff59fe1d62009d49a Mon Sep 17 00:00:00 2001 From: Ben Hers Date: Thu, 29 Oct 2026 09:23:04 -0700 Subject: [PATCH 1/3] Added a multi-client TCP server output transport. Added TCPBroadcastServerTransport, an output-only tcp://:PORT listener that sends to any number of simultaneously connected clients, dropping clients that disconnect or cannot keep up. create_transport() now returns it for tcp://:PORT when mode='output'. Co-Authored-By: Claude Sonnet 5.5 --- .../utils/transport_utils.py | 123 +++++++++++++++++- python/tests/test_tcp_broadcast_transport.py | 74 +++++++++++ 2 files changed, 195 insertions(+), 2 deletions(-) create mode 100644 python/tests/test_tcp_broadcast_transport.py diff --git a/python/fusion_engine_client/utils/transport_utils.py b/python/fusion_engine_client/utils/transport_utils.py index 5215d932..877fb1e7 100644 --- a/python/fusion_engine_client/utils/transport_utils.py +++ b/python/fusion_engine_client/utils/transport_utils.py @@ -365,6 +365,117 @@ def _deferred(*args, **kwargs): return getattr(self._socket, name) +class TCPBroadcastServerTransport: + """! + @brief Output-only TCP transport that sends to any number of simultaneously connected clients. + + This class starts listening immediately and accepts connections on a background thread, so construction does not + block. Clients may connect and disconnect at any time. `send()` writes the data to every connected client; it does + not block waiting for a client, and the data is silently dropped if no client is connected. A client that + disconnects, errors, or cannot keep up (a send takes longer than `_SEND_TIMEOUT_SEC`) is dropped without + affecting the others. + + Anything a client sends to this transport is ignored, and `recv()` is not supported. + """ + + # How often the background thread wakes up to check whether it should stop. + _POLL_INTERVAL_SEC = 0.2 + + # The longest a single client may stall `send()` before it is dropped. + _SEND_TIMEOUT_SEC = 0.5 + + _LISTEN_BACKLOG = 16 + + def __init__(self, port: int, print_func: Callable = None): + self._closed = False + self._print_func = print_func + self._lock = threading.Lock() + self._clients = {} + + self._listener = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self._listener.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + self._listener.settimeout(self._POLL_INTERVAL_SEC) + self._listener.bind(('', port)) + self._listener.listen(self._LISTEN_BACKLOG) + + self._accept_thread = threading.Thread( + target=self._accept_loop, name=f'tcp-broadcast-accept-{port}', daemon=True) + self._accept_thread.start() + + @property + def num_clients(self) -> int: + with self._lock: + return len(self._clients) + + def _accept_loop(self): + while True: + try: + client_socket, client_address = self._listener.accept() + except socket.timeout: + with self._lock: + if self._closed: + return + continue + except OSError: + return # Listener was closed out from under us by close(). + + client_socket.settimeout(self._SEND_TIMEOUT_SEC) + with self._lock: + if self._closed: + client_socket.close() + return + self._clients[client_socket] = client_address + + if self._print_func is not None: + self._print_func(f'Accepted connection from {client_address[0]}:{client_address[1]}.') + + def _drop_client(self, client_socket: socket.socket): + with self._lock: + client_address = self._clients.pop(client_socket, None) + client_socket.close() + if client_address is not None and self._print_func is not None: + self._print_func(f'Client {client_address[0]}:{client_address[1]} disconnected.') + + def send(self, data, *args, **kwargs) -> int: + """! + @brief Send data to all connected clients. + + @param data The data to be sent. + + @return The number of bytes sent to each client that received it, or 0 if no client is connected. + """ + with self._lock: + clients = list(self._clients) + + for client_socket in clients: + try: + client_socket.sendall(data, *args, **kwargs) + except OSError: + self._drop_client(client_socket) + return len(data) if len(clients) > 0 else 0 + + def recv(self, *args, **kwargs) -> bytes: + raise NotImplementedError('TCPBroadcastServerTransport is output-only.') + + def close(self): + with self._lock: + if self._closed: + return + self._closed = True + clients = list(self._clients) + self._clients.clear() + self._listener.close() + self._accept_thread.join(timeout=self._POLL_INTERVAL_SEC * 2) + for client_socket in clients: + client_socket.close() + + def __enter__(self): + return self + + def __exit__(self, *args): + self.close() + + class WebsocketTransport: """! @brief Websocket wrapper class, mimicking the Python socket API. @@ -427,7 +538,8 @@ def __setattr__(self, item: str, value: Any) -> None: - tcp://HOSTNAME[:PORT] - Connect to the specified hostname (or IP address) and port over TCP (e.g., tty://192.168.0.3:30202); defaults to port 30200. - tcp://:PORT - Listen for an incoming TCP connection on the specified port - (e.g., tcp://:30200). + (e.g., tcp://:30200). When used for output only, any number of clients may + connect and all of them receive the data. - udp://:PORT - Listen for incoming data on the specified UDP port (e.g., udp://:12345). Note: When using UDP, you must configure the device to send data to your @@ -446,7 +558,7 @@ def __setattr__(self, item: str, value: Any) -> None: {TRANSPORT_HELP_OPTIONS} """ -TransportClass = Union[SocketTransport, serial.Serial, WebsocketTransport, FileTransport] +TransportClass = Union[SocketTransport, TCPBroadcastServerTransport, serial.Serial, WebsocketTransport, FileTransport] def create_transport(descriptor: str, timeout_sec: float = None, print_func: Callable = None, mode: str = 'both', @@ -490,6 +602,13 @@ def create_transport(descriptor: str, timeout_sec: float = None, print_func: Cal m = re.match(r'^tcp://(?:0\.0\.0\.0)?:([0-9]+)$', descriptor) if m: port = int(m.group(1)) + + # An output-only transport can serve any number of simultaneous clients, so every client receives the data. + if mode == 'output': + if print_func is not None: + print_func(f'Listening for TCP connections on port {port} (output to all connected clients).') + return TCPBroadcastServerTransport(port, print_func=print_func) + if print_func is not None: print_func(f'Listening for TCP connections on port {port}.') diff --git a/python/tests/test_tcp_broadcast_transport.py b/python/tests/test_tcp_broadcast_transport.py new file mode 100644 index 00000000..9c499fce --- /dev/null +++ b/python/tests/test_tcp_broadcast_transport.py @@ -0,0 +1,74 @@ +import socket +import time + +import pytest + +from fusion_engine_client.utils.transport_utils import TCPBroadcastServerTransport, create_transport + + +def _free_port(): + with socket.socket() as s: + s.bind(('', 0)) + return s.getsockname()[1] + + +def _wait_for_clients(transport, count, timeout_sec=2.0): + end = time.time() + timeout_sec + while transport.num_clients != count and time.time() < end: + time.sleep(0.01) + assert transport.num_clients == count + + +def _connect(port): + client = socket.create_connection(('127.0.0.1', port), timeout=2.0) + return client + + +@pytest.fixture +def transport(): + port = _free_port() + t = create_transport(f'tcp://:{port}', mode='output') + assert isinstance(t, TCPBroadcastServerTransport) + t.port = port + yield t + t.close() + + +def test_send_without_clients_is_dropped(transport): + assert transport.send(b'abc') == 0 + + +def test_all_clients_receive(transport): + clients = [_connect(transport.port) for _ in range(3)] + _wait_for_clients(transport, 3) + transport.send(b'hello') + for c in clients: + assert c.recv(16) == b'hello' + c.close() + + +def test_clients_come_and_go(transport): + a = _connect(transport.port) + _wait_for_clients(transport, 1) + transport.send(b'1') + assert a.recv(16) == b'1' + + b = _connect(transport.port) + _wait_for_clients(transport, 2) + a.close() + # The disconnect is detected on a subsequent send. + for _ in range(5): + transport.send(b'2') + time.sleep(0.05) + _wait_for_clients(transport, 1) + assert b.recv(16).startswith(b'2') + b.close() + + +def test_input_mode_still_single_client(): + port = _free_port() + t = create_transport(f'tcp://:{port}', mode='input') + try: + assert not isinstance(t, TCPBroadcastServerTransport) + finally: + t.close() From f1341c62e4c4b57efe325d054f43c313b9d3fa35 Mon Sep 17 00:00:00 2001 From: Ben Hers Date: Thu, 29 Oct 2026 09:23:04 -0700 Subject: [PATCH 2/3] Moved the pose relay helpers into utils/pose_relay.py. Moved transform_pose_to_target() and transformed_pose_to_external_pose_input() out of the pose_relay.py example so they can be shared with other tools, and added unit tests. Co-Authored-By: Claude Sonnet 5.5 --- python/examples/pose_relay.py | 179 +-------------- .../fusion_engine_client/utils/pose_relay.py | 205 ++++++++++++++++++ python/tests/test_pose_relay.py | 78 +++++++ 3 files changed, 286 insertions(+), 176 deletions(-) create mode 100644 python/fusion_engine_client/utils/pose_relay.py create mode 100644 python/tests/test_pose_relay.py diff --git a/python/examples/pose_relay.py b/python/examples/pose_relay.py index ebb1d989..0ee08593 100644 --- a/python/examples/pose_relay.py +++ b/python/examples/pose_relay.py @@ -11,7 +11,6 @@ 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 @@ -19,189 +18,17 @@ 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 diff --git a/python/fusion_engine_client/utils/pose_relay.py b/python/fusion_engine_client/utils/pose_relay.py new file mode 100644 index 00000000..19542e3f --- /dev/null +++ b/python/fusion_engine_client/utils/pose_relay.py @@ -0,0 +1,205 @@ +from typing import Dict + +import numpy as np +import pymap3d as pm + +from ..analysis.attitude import dcm_to_euler_angles, euler_angles_to_dcm, get_enu_rotation_matrix +from ..messages import ExternalPoseInput, PoseMessage, SystemTimeSource, Timestamp + +# Coordinate frame abbreviations: +# enu - Local navigation frame (East, North, Up) +# ecef - Earth-Centered, Earth-Fixed frame +# sb - Source body frame +# tb - Target body frame + + +def calculate_ypr_enu_to_tb_deg(ypr_enu_to_sb_deg: np.ndarray, rotmat_tb_to_sb: np.ndarray) -> np.ndarray: + """! + @brief Calculate the target body attitude in the local ENU frame. + + @param ypr_enu_to_sb_deg The source body attitude in the ENU frame (YPR, degrees). + @param rotmat_tb_to_sb The rotation matrix from the target body frame to the source body frame. + + @return The target body attitude in the ENU frame (YPR, degrees). + """ + 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] + + +def calculate_jacobian_ypr_target_wrt_source(ypr_enu_to_sb_deg: np.ndarray, rotmat_tb_to_sb: np.ndarray, + delta_deg: float = 1e-6) -> np.ndarray: + """! + @brief Numerically approximate the Jacobian of the target YPR with respect to the source YPR using central + differences. + + @param ypr_enu_to_sb_deg The source body attitude in the ENU frame (YPR, degrees). + @param rotmat_tb_to_sb The rotation matrix from the target body frame to the source body frame. + @param delta_deg The perturbation applied to each source angle (degrees). + + @return A 3x3 Jacobian matrix. + """ + 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 + + +def transform_pose_to_target(source_pose_msg: PoseMessage, + source_lever_arm_m: np.ndarray, target_lever_arm_m: np.ndarray, + ypr_tb_to_sb_deg: np.ndarray, vec_tb_to_sb_m: np.ndarray) -> Dict[str, np.ndarray]: + """! + @brief Transform a source device's pose to the corresponding pose of a target device. + + Any unavailable (NaN) fields in the source pose are passed through as NaN in the result. + + @param source_pose_msg The @ref PoseMessage from the source device. + @param source_lever_arm_m The source device's output lever arm in the source body frame (meters). + @param target_lever_arm_m The target device's output lever arm in the target body frame (meters). + @param ypr_tb_to_sb_deg The orientation of the target body frame relative to the source body frame (YPR, degrees). + @param vec_tb_to_sb_m The translation of the target body frame relative to the source body frame (meters). + + @return A `dict` containing the target position/attitude/velocity and their standard deviations. + """ + 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. + + 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, + } + + +def transformed_pose_to_external_pose_input(source_pose_msg: PoseMessage, transformed_pose: Dict[str, np.ndarray], + reset_engine_position: bool = True) -> ExternalPoseInput: + """! + @brief Package a transformed pose (see @ref transform_pose_to_target()) as an @ref ExternalPoseInput message. + + @param source_pose_msg The @ref PoseMessage from the source device, used for timing and solution type. + @param transformed_pose The result from @ref transform_pose_to_target(). + @param reset_engine_position If `True`, set `FLAG_RESET_POSITION_DATA` so the target resets its position using this + pose. + + @return The populated @ref ExternalPoseInput message. + """ + 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 if reset_engine_position else 0 + + 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 diff --git a/python/tests/test_pose_relay.py b/python/tests/test_pose_relay.py new file mode 100644 index 00000000..26e2a752 --- /dev/null +++ b/python/tests/test_pose_relay.py @@ -0,0 +1,78 @@ +import numpy as np +import pymap3d as pm +import pytest + +from fusion_engine_client.messages import ExternalPoseInput, PoseMessage, SolutionType, SystemTimeSource, Timestamp +from fusion_engine_client.utils.pose_relay import transform_pose_to_target, transformed_pose_to_external_pose_input + +ZERO = np.zeros(3) + + +def _make_valid_pose(): + pose = PoseMessage() + pose.p1_time = Timestamp(10.0) + pose.gps_time = Timestamp(1000.0) + pose.solution_type = SolutionType.RTKFixed + pose.lla_deg = np.array([37.7749, -122.4194, 10.0]) + pose.position_std_enu_m = np.array([0.1, 0.2, 0.3]) + pose.ypr_deg = np.array([45.0, 1.0, 2.0]) + pose.ypr_std_deg = np.array([0.5, 0.1, 0.1]) + pose.velocity_body_mps = np.array([3.0, 0.0, 0.0]) + pose.velocity_std_body_mps = np.array([0.05, 0.05, 0.05]) + return pose + + +class TestTransformPoseToTarget: + def test_identity_transform(self): + pose = _make_valid_pose() + result = transform_pose_to_target(pose, ZERO, ZERO, ZERO, ZERO) + + np.testing.assert_allclose(result['position_ecef_m'], pm.geodetic2ecef(*pose.lla_deg), atol=1e-6) + np.testing.assert_allclose(result['ypr_deg'], pose.ypr_deg) + np.testing.assert_allclose(result['ypr_std_deg'], pose.ypr_std_deg) + # Rotating the body velocity into ENU should not change its magnitude. + assert np.linalg.norm(result['velocity_enu_mps']) == pytest.approx(3.0) + # Rotating the position covariance into ECEF should preserve its total variance. + assert np.sum(result['position_std_ecef_m'] ** 2) == pytest.approx(np.sum(pose.position_std_enu_m ** 2)) + + def test_matching_lever_arms_cancel(self): + pose = _make_valid_pose() + lever_arm_m = np.array([1.0, -0.5, 0.8]) + result = transform_pose_to_target(pose, lever_arm_m, lever_arm_m, ZERO, ZERO) + np.testing.assert_allclose(result['position_ecef_m'], pm.geodetic2ecef(*pose.lla_deg), atol=1e-6) + + def test_target_lever_arm_offsets_position(self): + pose = _make_valid_pose() + result = transform_pose_to_target(pose, ZERO, np.array([1.0, 0.0, 0.0]), ZERO, ZERO) + offset_m = result['position_ecef_m'] - np.array(pm.geodetic2ecef(*pose.lla_deg)) + assert np.linalg.norm(offset_m) == pytest.approx(1.0) + + def test_invalid_pose_passes_through_nan(self): + result = transform_pose_to_target(PoseMessage(), ZERO, ZERO, np.array([90.0, 0.0, 0.0]), ZERO) + for key, value in result.items(): + assert np.all(np.isnan(value)), key + + +class TestTransformedPoseToExternalPoseInput: + def test_valid_pose(self): + pose = _make_valid_pose() + ext_pose = transformed_pose_to_external_pose_input(pose, transform_pose_to_target(pose, ZERO, ZERO, ZERO, ZERO)) + assert ext_pose.solution_type == SolutionType.RTKFixed + assert ext_pose.details.measurement_time_source == SystemTimeSource.GPS_TIME + assert float(ext_pose.details.measurement_time) == pytest.approx(1000.0) + assert ext_pose.flags == ExternalPoseInput.FLAG_RESET_POSITION_DATA + + def test_reset_disabled(self): + pose = _make_valid_pose() + ext_pose = transformed_pose_to_external_pose_input(pose, transform_pose_to_target(pose, ZERO, ZERO, ZERO, ZERO), + reset_engine_position=False) + assert ext_pose.flags == 0 + + def test_invalid_pose(self): + pose = PoseMessage() + ext_pose = transformed_pose_to_external_pose_input(pose, transform_pose_to_target(pose, ZERO, ZERO, ZERO, ZERO)) + assert ext_pose.solution_type == SolutionType.Invalid + assert ext_pose.details.measurement_time_source == SystemTimeSource.INVALID + assert np.all(np.isnan(ext_pose.position_ecef_m)) + # Must still serialize cleanly so it can be forwarded. + ext_pose.pack() From 8959119a8f70f260743749af4ec46023fc12412c Mon Sep 17 00:00:00 2001 From: Ben Hers Date: Thu, 29 Oct 2026 09:23:04 -0700 Subject: [PATCH 3/3] Added the p1_proxy tool to forward measurements between devices. p1_proxy forwards raw wheel speed, vehicle speed, IMU, and pose output from a source device to one or more targets as the corresponding input messages. Pose is transformed for the target's lever arm and mounting, and optionally resets the target's position on the first pose (--reset-target-position). Targets that are unreachable or disconnect are retried without stopping the proxy, and tcp://: targets accept any number of clients. Replaces the proxy_wheel_speed.py example. Co-Authored-By: Claude Sonnet 5.5 --- python/README.md | 7 + python/examples/proxy_wheel_speed.py | 235 --------- .../applications/p1_proxy.py | 460 ++++++++++++++++++ python/setup.py | 1 + python/tests/test_p1_proxy.py | 392 +++++++++++++++ 5 files changed, 860 insertions(+), 235 deletions(-) delete mode 100755 python/examples/proxy_wheel_speed.py create mode 100644 python/fusion_engine_client/applications/p1_proxy.py create mode 100644 python/tests/test_p1_proxy.py diff --git a/python/README.md b/python/README.md index a51211e5..5df4a4b2 100644 --- a/python/README.md +++ b/python/README.md @@ -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://:` 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 diff --git a/python/examples/proxy_wheel_speed.py b/python/examples/proxy_wheel_speed.py deleted file mode 100755 index 60026281..00000000 --- a/python/examples/proxy_wheel_speed.py +++ /dev/null @@ -1,235 +0,0 @@ -#!/usr/bin/env python3 - -"""Proxy raw wheel/vehicle speed output from a source device to a target device as wheel/vehicle speed input. - -This is useful when a vehicle has a "primary" device with a good connection to the vehicle's wheel speed or CAN -data (the source), and one or more other devices (targets) that need that same speed data but are not wired to -the vehicle themselves. -""" - -from datetime import datetime -import os -import sys -import time - -# 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.messages import * -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, ExtendedBooleanAction -from fusion_engine_client.utils.time_provider import TimeProvider -from fusion_engine_client.utils.transport_utils import * - -logger = logging.getLogger('point_one.fusion_engine.proxy_wheel_speed') - -# Map each raw output message we forward to the corresponding input message class sent to the target device. -_INPUT_CLASS_BY_OUTPUT_TYPE = { - MessageType.RAW_WHEEL_SPEED_OUTPUT: WheelSpeedInput, - MessageType.RAW_VEHICLE_SPEED_OUTPUT: VehicleSpeedInput, -} - -# Fields to copy directly from the source output message to the target input message. Both message types share -# the "gear" and "flags" fields; the speed field name(s) differ between wheel and vehicle speed messages. -_COPY_FIELDS_BY_OUTPUT_TYPE = { - MessageType.RAW_WHEEL_SPEED_OUTPUT: ( - 'front_left_speed_mps', 'front_right_speed_mps', 'rear_left_speed_mps', 'rear_right_speed_mps', 'gear', - 'flags'), - MessageType.RAW_VEHICLE_SPEED_OUTPUT: ('vehicle_speed_mps', 'gear', 'flags'), -} - - -def build_input_message(source_message, use_gps_time, time_provider): - """! - @brief Convert a raw wheel/vehicle speed output message from the source device into the corresponding input - message for the target device. - - Note that we intentionally do _not_ copy the source device's P1 time: P1 time is specific to each device and - is not shared between them. Instead, we either let the target device timestamp the measurement when it is - received, or, if requested, convert the source P1 time to GPS time so the target can align it more precisely with - its own P1 time. - - @param source_message The received `RawWheelSpeedOutput` or `RawVehicleSpeedOutput` message. - @param use_gps_time If `True`, timestamp the input message using GPS time derived from the source device's P1 - time. Otherwise, leave the timestamp unset so the target device timestamps it on reception. - @param time_provider A `TimeProvider` used to convert source P1 time to GPS time when `use_gps_time` is - enabled. - - @return The populated input message, or `None` if the message type is not supported. - """ - input_class = _INPUT_CLASS_BY_OUTPUT_TYPE.get(source_message.get_type()) - if input_class is None: - return None - - input_message = input_class() - for field in _COPY_FIELDS_BY_OUTPUT_TYPE[source_message.get_type()]: - setattr(input_message, field, getattr(source_message, field)) - - input_message.details.data_source = source_message.details.data_source - - gps_time = None - if use_gps_time: - if source_message.details.measurement_time_source == SystemTimeSource.GPS_TIME: - gps_time = source_message.details.measurement_time - else: - gps_time = time_provider.p1_to_gps(source_message.p1_time) - - if gps_time: - input_message.details.measurement_time = gps_time - input_message.details.measurement_time_source = SystemTimeSource.GPS_TIME - else: - if use_gps_time: - logger.debug('GPS time not yet available. Falling back to timestamp on reception.') - input_message.details.measurement_time_source = SystemTimeSource.INVALID - - return input_message - - -def main(): - parser = ArgumentParser( - usage="%(prog)s SOURCE TARGET [TARGET...]", - description="""\ -Proxy raw wheel/vehicle speed output from a source device to a target device as wheel/vehicle speed input. - -Connects to a source device and one or more target devices, enables raw wheel and vehicle speed output on the -source, and forwards each received measurement to every target as the corresponding input message. - -Messages may be timestamped either using GPS time (default) or on reception by the target device (--use-gps-time=false). - -Examples: - # Proxy to a single target. - ./proxy_wheel_speed.py tcp://192.168.1.100:30202 tcp://192.168.1.200:30201 - - # Proxy to multiple targets. - ./proxy_wheel_speed.py tcp://192.168.1.100:30202 tcp://192.168.1.200:30201 tcp://192.168.1.201:30201 -""") - - parser.add_argument( - 'source', type=str, - help="Source device transport (provides raw wheel/vehicle speed output).\n" + TRANSPORT_HELP_STRING) - - parser.add_argument( - 'target', type=str, nargs='+', - help="One or more target device transports (each receives wheel/vehicle speed input).\n" + - TRANSPORT_HELP_STRING) - - parser.add_argument( - '--use-gps-time', action=ExtendedBooleanAction, default=True, - help="""\ -Timestamp forwarded measurements using GPS time instead of letting the target timestamp them on reception.""") - - parser.add_argument( - '--summary-interval', type=float, default=5.0, - help="Print a summary of forwarded messages every N seconds.") - - parser.add_argument( - '-v', '--verbose', action='count', default=0, - help="Print verbose/trace debugging messages.") - - options = parser.parse_args() - - # Configure logging. - logging.basicConfig( - format='%(asctime)s - %(levelname)s - %(name)s:%(lineno)d - %(message)s', - stream=sys.stdout) - - if options.verbose == 0: - logger.setLevel(logging.INFO) - elif options.verbose == 1: - logger.setLevel(logging.DEBUG) - else: - logger.setLevel(logging.DEBUG) - logging.getLogger('point_one.fusion_engine.parsers').setLevel( - logging.getTraceLevel(depth=options.verbose - 1)) - - # Connect to the source and target devices. - response_timeout_sec = 3.0 - try: - source_transport = create_transport(options.source, timeout_sec=response_timeout_sec, print_func=logger.info) - except Exception as e: - logger.error(f"Failed to connect to source: {e}") - sys.exit(1) - - target_transports = [] - for target in options.target: - try: - target_transports.append( - create_transport(target, timeout_sec=response_timeout_sec, print_func=logger.info)) - except Exception as e: - logger.error(f"Failed to connect to target '{target}': {e}") - sys.exit(1) - - encoder = FusionEngineEncoder() - - # Ask the source device to send us raw wheel/vehicle speed output every time a new measurement is available. If - # requested, also enable pose output so we can convert the source's P1 time to GPS time. - message_types_to_enable = [MessageType.RAW_WHEEL_SPEED_OUTPUT, MessageType.RAW_VEHICLE_SPEED_OUTPUT] - if options.use_gps_time: - message_types_to_enable.append(MessageType.POSE) - - for message_type in message_types_to_enable: - rate_request = SetMessageRate(output_interface=InterfaceID(TransportType.CURRENT), - protocol=ProtocolType.FUSION_ENGINE, message_id=message_type, - rate=MessageRate.ON_CHANGE) - source_transport.send(encoder.encode_message(rate_request)) - - logger.info("Connected. Waiting for wheel/vehicle speed data from source...") - - # Main loop. - decoder = FusionEngineDecoder(return_bytes=False) - time_provider = TimeProvider() - - messages_received = 0 - messages_forwarded = 0 - start_time = datetime.now() - last_summary_time = start_time - - try: - while True: - # Need to specify a read size or read waits for end of file. This returns immediately even if 0 bytes - # are available. - received_data = recv_from_transport(source_transport, 64) - if len(received_data) == 0: - time.sleep(0.1) - else: - for _, message in decoder.on_data(received_data): - if isinstance(message, PoseMessage): - time_provider.handle_message(message) - continue - elif not isinstance(message, (RawWheelSpeedOutput, RawVehicleSpeedOutput)): - continue - - messages_received += 1 - - input_message = build_input_message(message, use_gps_time=options.use_gps_time, - time_provider=time_provider) - encoded_data = encoder.encode_message(input_message) - for target_transport in target_transports: - target_transport.send(encoded_data) - messages_forwarded += 1 - - logger.debug(f"Forwarded {message.get_type().name} -> {input_message.get_type().name} to " - f"{len(target_transports)} target(s).") - - now = datetime.now() - if (now - last_summary_time).total_seconds() >= options.summary_interval: - elapsed_sec = (now - start_time).total_seconds() - logger.info(f"Elapsed: {elapsed_sec:.1f} sec, received: {messages_received}, " - f"forwarded: {messages_forwarded}") - last_summary_time = now - except KeyboardInterrupt: - pass - except (ConnectionError, OSError) as e: - logger.error(f"Connection error: {e}") - finally: - source_transport.close() - for target_transport in target_transports: - target_transport.close() - logger.info(f"Done. Forwarded {messages_forwarded} of {messages_received} received messages.") - - -if __name__ == "__main__": - main() diff --git a/python/fusion_engine_client/applications/p1_proxy.py b/python/fusion_engine_client/applications/p1_proxy.py new file mode 100644 index 00000000..684aa166 --- /dev/null +++ b/python/fusion_engine_client/applications/p1_proxy.py @@ -0,0 +1,460 @@ +#!/usr/bin/env python3 + +import argparse +from argparse import Namespace +from datetime import datetime +import sys +import threading +import time +from typing import Callable, Dict, List, Optional, Set, Tuple, Type + +import numpy as np + +if __package__ is None or __package__ == "": + from import_utils import enable_relative_imports + __package__ = enable_relative_imports(__name__, __file__) + +from ..messages import * +from ..parsers import FusionEngineDecoder, FusionEngineEncoder +from ..utils import trace as logging +from ..utils.argument_parser import ArgumentParser, ExtendedBooleanAction +from ..utils.pose_relay import transform_pose_to_target, transformed_pose_to_external_pose_input +from ..utils.time_provider import TimeProvider +from ..utils.trace import BrokenPipeStreamHandler, HighlightFormatter +from ..utils.transport_utils import * + +_logger = logging.getLogger('point_one.fusion_engine.applications.p1_proxy') + +# Map each raw output message we forward to the corresponding input message class sent to the target device. +_INPUT_CLASS_BY_OUTPUT_TYPE: Dict[MessageType, Type[MessagePayload]] = { + MessageType.RAW_WHEEL_SPEED_OUTPUT: WheelSpeedInput, + MessageType.RAW_VEHICLE_SPEED_OUTPUT: VehicleSpeedInput, + MessageType.RAW_IMU_OUTPUT: IMUInput, + MessageType.POSE: ExternalPoseInput, +} + +# Fields copied from each raw output message to its corresponding input message. +_COPY_FIELDS_BY_OUTPUT_TYPE: Dict[MessageType, Tuple[str, ...]] = { + MessageType.RAW_WHEEL_SPEED_OUTPUT: ( + 'front_left_speed_mps', 'front_right_speed_mps', 'rear_left_speed_mps', 'rear_right_speed_mps', 'gear', + 'flags'), + MessageType.RAW_VEHICLE_SPEED_OUTPUT: ('vehicle_speed_mps', 'gear', 'flags'), + MessageType.RAW_IMU_OUTPUT: ('temperature_degc', 'accel_mps2', 'gyro_rps'), +} + +# Forwardable message types, by name. +_MESSAGE_TYPE_BY_NAME: Dict[str, MessageType] = { + 'WheelSpeedOutput': MessageType.RAW_WHEEL_SPEED_OUTPUT, + 'VehicleSpeedOutput': MessageType.RAW_VEHICLE_SPEED_OUTPUT, + 'IMUOutput': MessageType.RAW_IMU_OUTPUT, + 'Pose': MessageType.POSE, +} + +# Shorthand aliases accepted in addition to the full message names above. +_MESSAGE_TYPE_SHORTHANDS: Dict[str, MessageType] = { + 'wheel': MessageType.RAW_WHEEL_SPEED_OUTPUT, + 'vehicle': MessageType.RAW_VEHICLE_SPEED_OUTPUT, + 'imu': MessageType.RAW_IMU_OUTPUT, + 'pose': MessageType.POSE, +} + + +def parse_message_types(value: str) -> Set[MessageType]: + """! + @brief Parse a comma-separated list of message type names to forward. + + @param value The raw command-line string, e.g. "WheelSpeedOutput,IMUOutput" or the shorthand "wheel,imu". Names + are case-insensitive. 'all' or '' selects every supported type. + + @return The set of `MessageType`s to forward. + """ + if value.strip().lower() in ('all', ''): + return set(_MESSAGE_TYPE_BY_NAME.values()) + + types_by_name = {name.lower(): message_type for name, message_type in _MESSAGE_TYPE_BY_NAME.items()} + types_by_name.update(_MESSAGE_TYPE_SHORTHANDS) + + selected = set() + for name in value.split(','): + name = name.strip() + message_type = types_by_name.get(name.lower()) + if message_type is None: + valid = ', '.join(list(_MESSAGE_TYPE_BY_NAME.keys()) + list(_MESSAGE_TYPE_SHORTHANDS.keys())) + raise argparse.ArgumentTypeError(f"unsupported message type '{name}'. Valid options: {valid}, or 'all'.") + selected.add(message_type) + return selected + + +def parse_vec3(value: str) -> np.ndarray: + """! + @brief Parse a comma-separated "X,Y,Z" string into a 3-element float array. + + @param value The raw command-line string, e.g. "1.0,2.0,3.0". + + @return A 3-element `np.ndarray` of floats. + """ + parts = value.split(',') + if len(parts) != 3: + raise argparse.ArgumentTypeError(f"expected 3 comma-separated numbers (e.g. '1.0,2.0,3.0'), got '{value}'") + try: + return np.array([float(p) for p in parts]) + except ValueError: + raise argparse.ArgumentTypeError(f"expected 3 comma-separated numbers (e.g. '1.0,2.0,3.0'), got '{value}'") + + +def build_input_message(source_message: MessagePayload, options: Namespace, time_provider: TimeProvider, + reset_engine_position: bool = False) -> Optional[MessagePayload]: + """! + @brief Convert a raw output message from the source device into the corresponding input message for the target + device. + + Note that we intentionally do _not_ copy the source device's P1 time: P1 time is specific to each device and + is not shared between them. Instead, we either let the target device timestamp the measurement when it is + received, or, if requested, convert the source P1 time to GPS time so the target can align it more precisely with + its own P1 time. + + A pose message is transformed to the target device's lever arm and mounting (see the pose options) and always uses + the source's GPS time. + + @param source_message The received raw output message (see `_INPUT_CLASS_BY_OUTPUT_TYPE` for supported types). + @param options The parsed command line options. If `options.use_gps_time` is `True`, timestamp the input message + using GPS time derived from the source device's P1 time. Otherwise, leave the timestamp unset so the target + device timestamps it on reception. Pose messages are transformed using `options.source_lever_arm`, + `options.target_lever_arm`, `options.target_rotation`, and `options.target_translation`. + @param time_provider A `TimeProvider` used to convert source P1 time to GPS time when `options.use_gps_time` is + enabled. + @param reset_engine_position If `True`, a pose input message requests that the target device reset its position + using the pose. Ignored for other message types. + + @return The populated input message, or `None` if the message type is not supported. + """ + input_class = _INPUT_CLASS_BY_OUTPUT_TYPE.get(source_message.get_type()) + if input_class is None: + return None + + if source_message.get_type() == MessageType.POSE: + transformed_pose = transform_pose_to_target( + source_message, options.source_lever_arm, options.target_lever_arm, options.target_rotation, + options.target_translation) + return transformed_pose_to_external_pose_input( + source_message, transformed_pose, reset_engine_position=reset_engine_position) + + input_message = input_class() + for field in _COPY_FIELDS_BY_OUTPUT_TYPE[source_message.get_type()]: + setattr(input_message, field, getattr(source_message, field)) + + input_message.details.data_source = source_message.details.data_source + + gps_time = None + if options.use_gps_time: + if source_message.details.measurement_time_source == SystemTimeSource.GPS_TIME: + gps_time = source_message.details.measurement_time + else: + gps_time = time_provider.p1_to_gps(source_message.p1_time) + + if gps_time: + input_message.details.measurement_time = gps_time + input_message.details.measurement_time_source = SystemTimeSource.GPS_TIME + else: + if options.use_gps_time: + _logger.debug('GPS time not yet available. Falling back to timestamp on reception.') + input_message.details.measurement_time_source = SystemTimeSource.INVALID + + return input_message + + +class Target: + """! + @brief A target device that is connected in the background and reconnected automatically if it goes away. + + A target that fails to connect, or that fails while being sent to, does not affect the other targets. It is retried + every @ref _RETRY_INTERVAL_SEC until it comes back. Data is dropped for a target while it is disconnected. + """ + + _RETRY_INTERVAL_SEC = 5.0 + + def __init__(self, descriptor: str, timeout_sec: float): + self.descriptor = descriptor + self._timeout_sec = timeout_sec + self._transport: Optional[TransportClass] = None + self._lock = threading.Lock() + self._stop_event = threading.Event() + self._thread = threading.Thread(target=self._reconnect_loop, name=f'target-{descriptor}', daemon=True) + + @property + def connected(self) -> bool: + return self._transport is not None + + def start(self): + self._try_connect(print_func=_logger.info) + self._thread.start() + + def _try_connect(self, print_func: Callable = None): + try: + transport = create_transport(self.descriptor, timeout_sec=self._timeout_sec, print_func=print_func, + mode='output') + except Exception as e: + # Only log the first failure of an outage so we don't spam while a target is down. + if print_func is not None: + _logger.warning(f"Failed to connect to target '{self.descriptor}': {e}. " + f"Retrying every {self._RETRY_INTERVAL_SEC:g} sec.") + return + + with self._lock: + if self._stop_event.is_set(): + transport.close() + return + self._transport = transport + + if print_func is None: + _logger.info(f"Reconnected to target '{self.descriptor}'.") + + def _reconnect_loop(self): + while not self._stop_event.wait(self._RETRY_INTERVAL_SEC): + if self._transport is None: + self._try_connect() + + def send(self, data: bytes) -> bool: + """! + @brief Send data to the target if it is connected. + + @param data The data to send. + + @return `True` if the data was sent. + """ + transport = self._transport + if transport is None: + return False + + try: + transport.send(data) + except OSError as e: + with self._lock: + if self._transport is transport: + self._transport = None + transport.close() + _logger.warning(f"Lost connection to target '{self.descriptor}': {e}. " + f"Retrying every {self._RETRY_INTERVAL_SEC:g} sec.") + return False + + return True + + def close(self): + self._stop_event.set() + with self._lock: + transport, self._transport = self._transport, None + if transport is not None: + transport.close() + + +def main() -> None: + parser = ArgumentParser(description="""\ +Proxy raw measurement output from a source device to one or more targets as the corresponding input message. + +Connects to a source device and one or more targets, enables output for the messages to be proxied (e.g., wheel speed, +IMU), and forwards each received measurement to every target as the corresponding input message. Pose output is +forwarded as ExternalPoseInput, optionally transformed to account for the target's lever arm and mounting (see the Pose +options below). + +A target that is unreachable at startup, or that disconnects later, does not stop the proxy: it is retried every few +seconds and data is dropped for it in the meantime. The source is required, and the proxy exits if it is lost. + +Messages may be timestamped either using GPS time (default) or on reception by the target device (--use-gps-time=false). + +Examples: + # Proxy to a single target device. + ./p1_proxy.py tcp://192.168.1.100:30202 tcp://192.168.1.200:30201 + + # Proxy to multiple target devices. + ./p1_proxy.py tcp://192.168.1.100:30202 tcp://192.168.1.200:30201 tcp://192.168.1.201:30201 + + # Proxy only wheel speed and IMU measurements. + ./p1_proxy.py tcp://192.168.1.100:30202 tcp://192.168.1.200:30201 -m wheel,imu + + # Listen on port 30303 for any number of FusionEngine applications/devices to connect to this proxy, rather than + # connecting out to a target device. + ./p1_proxy.py tcp://192.168.1.100:30202 tcp://:30303 +""") + + shorthand_by_type = {message_type: name for name, message_type in _MESSAGE_TYPE_SHORTHANDS.items()} + supported_types = '\n'.join(f'- {name} ({shorthand_by_type[message_type]})' + for name, message_type in _MESSAGE_TYPE_BY_NAME.items()) + + parser.add_argument( + 'source', type=str, + help="Source device transport (provides raw measurement output).\n" + TRANSPORT_HELP_STRING) + parser.add_argument( + 'target', type=str, nargs='+', + help="One or more target transports (each receives measurement input). Use tcp://: to listen for " + "incoming connections instead of connecting out to a target; any number of clients may connect and " + "disconnect at any time, and all of them receive the data.\n" + TRANSPORT_HELP_STRING) + + parser.add_argument( + '-m', '--message-type', type=parse_message_types, action='append', + help="""\ +The message types to forward. May be specified multiple times (-m wheel -m imu), or as a comma-separated list +(-m wheel,imu). All matches are case-insensitive. If not specified, all supported types are forwarded. + +Supported types: +%s""" % supported_types) + parser.add_argument( + '--use-gps-time', action=ExtendedBooleanAction, default=True, + help="""\ +Timestamp forwarded wheel speed, vehicle speed, and IMU measurements using GPS time instead of letting the target +timestamp them on reception. Pose is always timestamped using the source's GPS time when available.""") + parser.add_argument( + '--summary-interval', type=float, default=5.0, + help="Print a summary of forwarded messages every N seconds.") + parser.add_argument( + '-v', '--verbose', action='count', default=0, + help="Print verbose/trace debugging messages.") + + pose_group = parser.add_argument_group('Pose') + pose_group.add_argument( + '--pose-rate', choices=('10hz', '100hz'), default='10hz', + help="""\ +Pose output rate requested from the source device: '10hz' (MessageRate.INTERVAL_100_MS) or '100hz' +(MessageRate.INTERVAL_10_MS).""") + pose_group.add_argument( + '--reset-target-position', action='store_true', + help="""\ +Set the reset position flag on the first pose forwarded after startup, causing each target to reset its position using +that pose. By default, poses are forwarded as normal updates without resetting the target.""") + pose_group.add_argument( + '--source-lever-arm', type=parse_vec3, default='0,0,0', metavar='X,Y,Z', + help="""\ +Output lever arm of the source device in the source vehicle's body frame (meters), as x_forward,y_left,z_up (e.g., +'0.5,0,1.0').""") + pose_group.add_argument( + '--target-lever-arm', type=parse_vec3, default='0,0,0', metavar='X,Y,Z', + help="""\ +Output lever arm of the target device in the target vehicle's body frame (meters), as x_forward,y_left,z_up. Should +match the target's own configured output lever arm.""") + pose_group.add_argument( + '--target-rotation', type=parse_vec3, default='0,0,0', metavar='YAW,PITCH,ROLL', + help="""\ +Orientation of the target device body frame relative to the source's body frame (YPR, degrees) -- how the target is +mounted on the source vehicle (e.g., '90,0,0').""") + pose_group.add_argument( + '--target-translation', type=parse_vec3, default='0,0,0', metavar='X,Y,Z', + help="""\ +Translation of the target device body frame relative to the source's body frame (meters, XYZ) -- how the target is +mounted on the source vehicle.""") + + options = parser.parse_args() + + if options.message_type: + message_types = set().union(*options.message_type) + else: + message_types = set(_MESSAGE_TYPE_BY_NAME.values()) + + # Configure logging. + if options.verbose >= 1: + logging.basicConfig(level=logging.INFO, format='%(levelname)s - %(name)s:%(lineno)d - %(message)s', + stream=sys.stdout) + _logger.setLevel(logging.DEBUG) + if options.verbose > 1: + logging.getLogger('point_one.fusion_engine.parsers').setLevel( + logging.getTraceLevel(depth=options.verbose - 1)) + else: + logging.basicConfig(level=logging.INFO, format='%(message)s', stream=sys.stdout) + + HighlightFormatter.install(color=True, standoff_level=logging.WARNING) + BrokenPipeStreamHandler.install() + + # Connect to the source device and each target. A tcp://: target listens in the background and does not + # block waiting for a client to connect. + response_timeout_sec = 3.0 + try: + source_transport: TransportClass = create_transport( + options.source, timeout_sec=response_timeout_sec, print_func=_logger.info) + except Exception as e: + _logger.error(f"Failed to connect to source: {e}") + sys.exit(1) + + # A target that fails to connect does not stop the proxy; it is retried in the background. + targets = [Target(descriptor, response_timeout_sec) for descriptor in options.target] + for target in targets: + target.start() + + encoder = FusionEngineEncoder() + + # Request raw measurement output from the source as each measurement becomes available, and pose output at + # --pose-rate. + for message_type in _MESSAGE_TYPE_BY_NAME.values(): + if message_type not in message_types: + continue + + if message_type == MessageType.POSE: + rate = MessageRate.INTERVAL_100_MS if options.pose_rate == '10hz' else MessageRate.INTERVAL_10_MS + else: + rate = MessageRate.ON_CHANGE + + rate_request = SetMessageRate(output_interface=InterfaceID(TransportType.CURRENT), + protocol=ProtocolType.FUSION_ENGINE, message_id=message_type, rate=rate) + source_transport.send(encoder.encode_message(rate_request)) + + _logger.info("Connected. Waiting for measurement data from source...") + + # Main loop. + decoder = FusionEngineDecoder(return_bytes=False) + time_provider = TimeProvider() + + messages_received = 0 + messages_forwarded = 0 + reset_position_pending = options.reset_target_position + start_time = datetime.now() + last_summary_time = start_time + + try: + while True: + # Need to specify a read size or read waits for end of file. This returns immediately even if 0 bytes + # are available. + received_data = recv_from_transport(source_transport, 64) + if len(received_data) == 0: + time.sleep(0.1) + else: + for _, message in decoder.on_data(received_data): + messages_received += 1 + + if isinstance(message, PoseMessage): + # Always track the source device P1/GPS time relationship, even if Pose isn't being + # forwarded, so other measurements can be timestamped in GPS time. + time_provider.handle_message(message) + + # Skip messages that weren't selected with --message-type. + if message.get_type() not in message_types: + continue + + input_message = build_input_message(message, options, time_provider, + reset_engine_position=reset_position_pending) + if isinstance(message, PoseMessage): + # Only the first pose forwarded after startup resets the targets. + reset_position_pending = False + + encoded_data = encoder.encode_message(input_message) + num_sent = sum(target.send(encoded_data) for target in targets) + messages_forwarded += 1 + + _logger.debug(f"Forwarded {message.get_type().name} -> {input_message.get_type().name} to " + f"{num_sent} of {len(targets)} target(s).") + + now = datetime.now() + if (now - last_summary_time).total_seconds() >= options.summary_interval: + elapsed_sec = (now - start_time).total_seconds() + _logger.info(f"Elapsed: {elapsed_sec:.1f} sec, received: {messages_received}, " + f"forwarded: {messages_forwarded}") + last_summary_time = now + except KeyboardInterrupt: + pass + except (ConnectionError, OSError) as e: + _logger.error(f"Connection error: {e}") + finally: + source_transport.close() + for target in targets: + target.close() + _logger.info(f"Done. Forwarded {messages_forwarded} of {messages_received} received messages.") + + +if __name__ == "__main__": + main() diff --git a/python/setup.py b/python/setup.py index 4f94f8b8..7bd9f853 100644 --- a/python/setup.py +++ b/python/setup.py @@ -92,6 +92,7 @@ def find_version(*file_paths): 'p1_filter = fusion_engine_client.applications.p1_filter:main', 'p1_lband_extract = fusion_engine_client.applications.p1_lband_extract:main', 'p1_print = fusion_engine_client.applications.p1_print:main', + 'p1_proxy = fusion_engine_client.applications.p1_proxy:main', ] }, python_requires='>=3.6', diff --git a/python/tests/test_p1_proxy.py b/python/tests/test_p1_proxy.py new file mode 100644 index 00000000..f480e5aa --- /dev/null +++ b/python/tests/test_p1_proxy.py @@ -0,0 +1,392 @@ +import argparse +import sys +import time + +import numpy as np +import pytest + +from fusion_engine_client.applications import p1_proxy +from fusion_engine_client.applications.p1_proxy import build_input_message, parse_message_types, parse_vec3 +from fusion_engine_client.messages import ( + EventNotificationMessage, + ExternalPoseInput, + GearType, + IMUInput, + MessageRate, + MessageType, + PoseMessage, + RawIMUOutput, + RawVehicleSpeedOutput, + RawWheelSpeedOutput, + SensorDataSource, + SolutionType, + SystemTimeSource, + Timestamp, + VehicleSpeedInput, + WheelSpeedInput, +) +from fusion_engine_client.parsers import FusionEngineDecoder, FusionEngineEncoder +from fusion_engine_client.utils.time_provider import TimeProvider + +ALL_TYPES = {MessageType.RAW_WHEEL_SPEED_OUTPUT, MessageType.RAW_VEHICLE_SPEED_OUTPUT, MessageType.RAW_IMU_OUTPUT, + MessageType.POSE} + + +class TestParseMessageTypes: + def test_full_names(self): + assert parse_message_types('WheelSpeedOutput,IMUOutput') == \ + {MessageType.RAW_WHEEL_SPEED_OUTPUT, MessageType.RAW_IMU_OUTPUT} + + def test_shorthands(self): + assert parse_message_types('wheel,vehicle,imu,pose') == ALL_TYPES + + def test_case_insensitive_and_whitespace(self): + assert parse_message_types(' WHEEL , imuoutput ') == \ + {MessageType.RAW_WHEEL_SPEED_OUTPUT, MessageType.RAW_IMU_OUTPUT} + + def test_mixed_full_and_shorthand(self): + assert parse_message_types('Pose,wheel') == {MessageType.POSE, MessageType.RAW_WHEEL_SPEED_OUTPUT} + + @pytest.mark.parametrize('value', ['all', 'ALL', '']) + def test_all(self, value): + assert parse_message_types(value) == ALL_TYPES + + @pytest.mark.parametrize('value', ['bogus', 'wheel,bogus', 'wheel,,imu']) + def test_invalid(self, value): + with pytest.raises(argparse.ArgumentTypeError): + parse_message_types(value) + + +class TestParseVec3: + def test_valid(self): + np.testing.assert_array_equal(parse_vec3('1.0,-2,3.5'), [1.0, -2.0, 3.5]) + + @pytest.mark.parametrize('value', ['1,2', '1,2,3,4', '1,a,3', '']) + def test_invalid(self, value): + with pytest.raises(argparse.ArgumentTypeError): + parse_vec3(value) + + +def _make_wheel_speed(p1_sec=10.0): + message = RawWheelSpeedOutput() + message.details.p1_time = Timestamp(p1_sec) + message.details.data_source = SensorDataSource.CAN + message.front_left_speed_mps = 1.0 + message.front_right_speed_mps = 2.0 + message.rear_left_speed_mps = 3.0 + message.rear_right_speed_mps = 4.0 + message.gear = GearType.FORWARD + message.flags = 0x1 + return message + + +def _make_time_provider(p1_sec, gps_sec): + pose = PoseMessage() + pose.p1_time = Timestamp(p1_sec) + pose.gps_time = Timestamp(gps_sec) + time_provider = TimeProvider() + time_provider.handle_message(pose) + return time_provider + + +def _options(**overrides): + options = argparse.Namespace(use_gps_time=False, source_lever_arm=np.zeros(3), target_lever_arm=np.zeros(3), + target_rotation=np.zeros(3), target_translation=np.zeros(3)) + for key, value in overrides.items(): + setattr(options, key, value) + return options + + +class TestBuildInputMessage: + def test_wheel_speed_fields_copied(self): + result = build_input_message(_make_wheel_speed(), _options(use_gps_time=False), TimeProvider()) + assert isinstance(result, WheelSpeedInput) + assert result.front_left_speed_mps == 1.0 + assert result.front_right_speed_mps == 2.0 + assert result.rear_left_speed_mps == 3.0 + assert result.rear_right_speed_mps == 4.0 + assert result.gear == GearType.FORWARD + assert result.flags == 0x1 + assert result.details.data_source == SensorDataSource.CAN + + def test_vehicle_speed_fields_copied(self): + message = RawVehicleSpeedOutput() + message.vehicle_speed_mps = 5.0 + message.gear = GearType.REVERSE + result = build_input_message(message, _options(use_gps_time=False), TimeProvider()) + assert isinstance(result, VehicleSpeedInput) + assert result.vehicle_speed_mps == 5.0 + assert result.gear == GearType.REVERSE + + def test_imu_fields_copied(self): + message = RawIMUOutput() + message.temperature_degc = 25.0 + message.accel_mps2 = np.array([0.1, 0.2, 9.8]) + message.gyro_rps = np.array([0.01, 0.02, 0.03]) + result = build_input_message(message, _options(use_gps_time=False), TimeProvider()) + assert isinstance(result, IMUInput) + assert result.temperature_degc == 25.0 + np.testing.assert_array_equal(result.accel_mps2, message.accel_mps2) + np.testing.assert_array_equal(result.gyro_rps, message.gyro_rps) + + def test_unsupported_type_returns_none(self): + assert build_input_message(EventNotificationMessage(), _options(), TimeProvider()) is None + + def test_pose_transformed_without_reset_by_default(self): + result = build_input_message(_make_pose(100.0, 1000.0), _options(), TimeProvider()) + assert isinstance(result, ExternalPoseInput) + assert result.flags == 0 + assert float(result.details.measurement_time) == pytest.approx(1000.0) + + def test_pose_reset_requested(self): + result = build_input_message(_make_pose(100.0, 1000.0), _options(), TimeProvider(), reset_engine_position=True) + assert result.flags == ExternalPoseInput.FLAG_RESET_POSITION_DATA + + def test_no_gps_time_leaves_timestamp_to_target(self): + time_provider = _make_time_provider(p1_sec=10.0, gps_sec=1000.0) + result = build_input_message(_make_wheel_speed(), _options(use_gps_time=False), time_provider) + assert result.details.measurement_time_source == SystemTimeSource.INVALID + + def test_gps_time_converted_from_p1(self): + time_provider = _make_time_provider(p1_sec=10.0, gps_sec=1000.0) + result = build_input_message(_make_wheel_speed(p1_sec=12.5), _options(use_gps_time=True), time_provider) + assert result.details.measurement_time_source == SystemTimeSource.GPS_TIME + assert float(result.details.measurement_time) == pytest.approx(1002.5) + + def test_gps_time_passed_through_from_source(self): + message = _make_wheel_speed() + message.details.measurement_time = Timestamp(2000.0) + message.details.measurement_time_source = SystemTimeSource.GPS_TIME + result = build_input_message(message, _options(use_gps_time=True), TimeProvider()) + assert result.details.measurement_time_source == SystemTimeSource.GPS_TIME + assert float(result.details.measurement_time) == pytest.approx(2000.0) + + def test_gps_time_unavailable_falls_back_to_reception(self): + result = build_input_message(_make_wheel_speed(), _options(use_gps_time=True), TimeProvider()) + assert result.details.measurement_time_source == SystemTimeSource.INVALID + + +SOURCE = 'tcp://source:30202' +TARGETS = ['tcp://target1:30201', 'tcp://target2:30201', 'tcp://:30303'] + + +class _FakeTransport: + """! + @brief Stand-in for a transport that records everything sent to it. + """ + def __init__(self): + self.sent = [] + self.closed = False + + def send(self, data): + self.sent.append(bytes(data)) + + def close(self): + self.closed = True + + def decode_sent(self): + return [message for _, message in FusionEngineDecoder().on_data(b''.join(self.sent))] + + +def _make_pose(p1_sec, gps_sec): + pose = PoseMessage() + pose.p1_time = Timestamp(p1_sec) + pose.gps_time = Timestamp(gps_sec) + pose.solution_type = SolutionType.RTKFixed + pose.lla_deg = np.array([37.7749, -122.4194, 10.0]) + pose.position_std_enu_m = np.array([0.1, 0.1, 0.2]) + pose.ypr_deg = np.array([90.0, 0.0, 0.0]) + pose.ypr_std_deg = np.array([0.5, 0.1, 0.1]) + return pose + + +def _make_source_stream(): + imu = RawIMUOutput() + imu.details.p1_time = Timestamp(10.06) + encoder = FusionEngineEncoder() + messages = [ + _make_pose(p1_sec=10.0, gps_sec=1000.0), + _make_wheel_speed(p1_sec=10.05), + imu, + # Not a forwardable type -- should be ignored. + EventNotificationMessage(), + _make_pose(p1_sec=10.1, gps_sec=1000.1), + ] + return b''.join(encoder.encode_message(m) for m in messages) + + +def _run_proxy(monkeypatch, extra_args=(), targets=TARGETS): + source = _FakeTransport() + target_transports = {} + + def fake_create_transport(descriptor, **kwargs): + if descriptor == SOURCE: + return source + target_transports[descriptor] = _FakeTransport() + return target_transports[descriptor] + + # Deliver the whole source stream in one read, then stop the proxy as if the user hit Ctrl-C. + chunks = [_make_source_stream()] + + def fake_recv_from_transport(transport, size): + if chunks: + return chunks.pop(0) + raise KeyboardInterrupt + + monkeypatch.setattr(p1_proxy, 'create_transport', fake_create_transport) + monkeypatch.setattr(p1_proxy, 'recv_from_transport', fake_recv_from_transport) + monkeypatch.setattr(sys, 'argv', ['p1_proxy', SOURCE, *targets, *extra_args]) + p1_proxy.main() + return source, target_transports + + +def _requested_rates(source): + return {MessageType(m.message_id): m.rate for m in source.decode_sent()} + + +class TestMain: + def test_forwards_to_every_target(self, monkeypatch): + source, targets = _run_proxy(monkeypatch) + + assert list(targets.keys()) == TARGETS + first = targets[TARGETS[0]] + for target in targets.values(): + assert target.sent == first.sent + assert target.closed + assert source.closed + + forwarded = first.decode_sent() + assert [m.get_type() for m in forwarded] == [ + MessageType.EXTERNAL_POSE_INPUT, + MessageType.WHEEL_SPEED_INPUT, + MessageType.IMU_INPUT, + MessageType.EXTERNAL_POSE_INPUT, + ] + + def test_pose_does_not_reset_target_by_default(self, monkeypatch): + _, targets = _run_proxy(monkeypatch, extra_args=['-m', 'pose']) + poses = targets[TARGETS[0]].decode_sent() + assert [p.flags for p in poses] == [0, 0] + + def test_only_first_pose_resets_target_when_requested(self, monkeypatch): + _, targets = _run_proxy(monkeypatch, extra_args=['-m', 'pose', '--reset-target-position']) + for target in targets.values(): + assert [p.flags for p in target.decode_sent()] == [ExternalPoseInput.FLAG_RESET_POSITION_DATA, 0] + + def test_requests_rates_from_source(self, monkeypatch): + source, _ = _run_proxy(monkeypatch) + assert _requested_rates(source) == { + MessageType.RAW_WHEEL_SPEED_OUTPUT: MessageRate.ON_CHANGE, + MessageType.RAW_VEHICLE_SPEED_OUTPUT: MessageRate.ON_CHANGE, + MessageType.RAW_IMU_OUTPUT: MessageRate.ON_CHANGE, + MessageType.POSE: MessageRate.INTERVAL_100_MS, + } + + def test_pose_rate_100hz(self, monkeypatch): + source, _ = _run_proxy(monkeypatch, extra_args=['--pose-rate=100hz']) + assert _requested_rates(source)[MessageType.POSE] == MessageRate.INTERVAL_10_MS + + def test_message_type_filter(self, monkeypatch): + source, targets = _run_proxy(monkeypatch, extra_args=['-m', 'wheel', '-m', 'Pose']) + + assert set(_requested_rates(source).keys()) == {MessageType.RAW_WHEEL_SPEED_OUTPUT, MessageType.POSE} + for target in targets.values(): + assert [m.get_type() for m in target.decode_sent()] == [ + MessageType.EXTERNAL_POSE_INPUT, + MessageType.WHEEL_SPEED_INPUT, + MessageType.EXTERNAL_POSE_INPUT, + ] + + def test_target_connection_failure_does_not_stop_proxy(self, monkeypatch): + source = _FakeTransport() + good = _FakeTransport() + + def fake_create_transport(descriptor, **kwargs): + if descriptor == SOURCE: + return source + elif descriptor == TARGETS[0]: + raise ConnectionRefusedError('refused') + else: + return good + + chunks = [_make_source_stream()] + + def fake_recv_from_transport(transport, size): + if chunks: + return chunks.pop(0) + raise KeyboardInterrupt + + monkeypatch.setattr(p1_proxy, 'create_transport', fake_create_transport) + monkeypatch.setattr(p1_proxy, 'recv_from_transport', fake_recv_from_transport) + monkeypatch.setattr(sys, 'argv', ['p1_proxy', SOURCE, TARGETS[0], TARGETS[1]]) + p1_proxy.main() + assert len(good.decode_sent()) == 4 + + +class _FailingTransport(_FakeTransport): + def __init__(self): + super().__init__() + self.fail = False + + def send(self, data): + if self.fail: + raise BrokenPipeError('gone') + super().send(data) + + +class TestTarget: + @pytest.fixture(autouse=True) + def fast_retry(self, monkeypatch): + monkeypatch.setattr(p1_proxy.Target, '_RETRY_INTERVAL_SEC', 0.01) + + @staticmethod + def _wait_until(condition, timeout_sec=2.0): + end = time.time() + timeout_sec + while not condition() and time.time() < end: + time.sleep(0.005) + assert condition() + + def test_failed_send_drops_target_and_reconnects(self, monkeypatch): + transports = [] + + def fake_create_transport(descriptor, **kwargs): + transports.append(_FailingTransport()) + return transports[-1] + + monkeypatch.setattr(p1_proxy, 'create_transport', fake_create_transport) + target = p1_proxy.Target('tcp://x:1', 1.0) + target.start() + try: + assert target.send(b'a') + assert target.send(b'b') + assert transports[0].sent == [b'a', b'b'] + + transports[0].fail = True + assert not target.send(b'c') + assert not target.connected + assert transports[0].closed + + self._wait_until(lambda: target.connected) + assert len(transports) == 2 + assert target.send(b'd') + assert transports[1].sent == [b'd'] + finally: + target.close() + + def test_initial_failure_is_retried(self, monkeypatch): + attempts = [] + + def fake_create_transport(descriptor, **kwargs): + attempts.append(1) + if len(attempts) < 3: + raise ConnectionRefusedError('refused') + return _FakeTransport() + + monkeypatch.setattr(p1_proxy, 'create_transport', fake_create_transport) + target = p1_proxy.Target('tcp://x:1', 1.0) + target.start() + try: + assert not target.send(b'a') + self._wait_until(lambda: target.connected) + finally: + target.close()