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/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/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/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/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/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() 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() 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()