diff --git a/.github/workflows/validate-external-sim-bridge.yml b/.github/workflows/validate-external-sim-bridge.yml new file mode 100644 index 0000000..3f95100 --- /dev/null +++ b/.github/workflows/validate-external-sim-bridge.yml @@ -0,0 +1,62 @@ +name: Validate external simulator bridge + +on: + push: + paths: + - 'src/external_sim_bridge/**' + - 'src/DistributedSatelliteSim/**' + - '.github/workflows/validate-external-sim-bridge.yml' + pull_request: + paths: + - 'src/external_sim_bridge/**' + - 'src/DistributedSatelliteSim/**' + - '.github/workflows/validate-external-sim-bridge.yml' + workflow_dispatch: + +jobs: + validate-bridge: + runs-on: ubuntu-latest + permissions: + contents: read + packages: read + + steps: + - name: Downcase repository name + run: | + echo "REPO=${GITHUB_REPOSITORY@L}" >> "${GITHUB_ENV}" + + - name: Checkout repository + uses: actions/checkout@v4 + + - name: Log in to GitHub Container Registry + uses: docker/login-action@v3 + with: + registry: ghcr.io + username: ${{ github.actor }} + password: ${{ secrets.GITHUB_TOKEN }} + + - name: Pull ROS 2 validation image + run: docker pull ghcr.io/${{ env.REPO }}:latest + + - name: Build and test external simulator bridge with Basilisk + env: + ASTRO_EXPECT_BASILISK: '1' + run: | + docker run --rm \ + --user root \ + -e ASTRO_EXPECT_BASILISK="${ASTRO_EXPECT_BASILISK}" \ + -v "${GITHUB_WORKSPACE}:/home/ws" \ + -w /home/ws \ + ghcr.io/${{ env.REPO }}:latest \ + bash -lc ' + set -e + source /opt/ros/kilted/setup.bash + python3 -m pip install --break-system-packages "bsk==2.10.0" + python3 -c "import Basilisk; print(Basilisk.__version__)" + rosdep update + rosdep install --from-paths src --ignore-src -y + colcon build --packages-select distributed_satellite_sim external_sim_bridge + source install/setup.bash + colcon test --packages-select external_sim_bridge + colcon test-result --verbose + ' diff --git a/.gitignore b/.gitignore index 908ae66..44ca08e 100755 --- a/.gitignore +++ b/.gitignore @@ -2,5 +2,7 @@ log/ *.log build/ install/ +__pycache__/ +*.py[cod] **/.DS_Store -**/.env \ No newline at end of file +**/.env diff --git a/src/DistributedSatelliteSim/CMakeLists.txt b/src/DistributedSatelliteSim/CMakeLists.txt index 1634c3a..a3c132c 100755 --- a/src/DistributedSatelliteSim/CMakeLists.txt +++ b/src/DistributedSatelliteSim/CMakeLists.txt @@ -86,6 +86,11 @@ install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch ) +install(FILES + test/data/dlqr_reference_trajectory.csv + DESTINATION share/${PROJECT_NAME}/test/data +) + install(DIRECTORY config/ DESTINATION share/${PROJECT_NAME}/config ) @@ -139,4 +144,4 @@ if(BUILD_TESTING) endif() -ament_package() \ No newline at end of file +ament_package() diff --git a/src/external_sim_bridge/doc/design_note.md b/src/external_sim_bridge/doc/design_note.md new file mode 100644 index 0000000..9bf0310 --- /dev/null +++ b/src/external_sim_bridge/doc/design_note.md @@ -0,0 +1,283 @@ +# External Simulator Bridge Design Note + +## Purpose + +This note defines the v1 contract for ASTRO's external simulator bridge under GH issue `#24`. +The goal is to validate the manuscript branch's external deployment pattern against Basilisk +without changing ASTRO's current ROS-facing control boundary. + +For v1, the bridge replaces `env_node` in the closed loop while leaving +`gnc_node` unchanged. + +## Current `main` Baseline + +The implementation baseline is the current `main` branch package at +`src/DistributedSatelliteSim`. + +The behavior to preserve is defined primarily by: + +- `src/DistributedSatelliteSim/include/distributed_satellite_sim/env_node.hpp` +- `src/DistributedSatelliteSim/include/distributed_satellite_sim/gnc_node.hpp` +- `src/DistributedSatelliteSim/launch/sim.launch.py` +- `src/DistributedSatelliteSim/test/test_dlqr_reference_launch.py` + +Key baseline facts: + +- `EnvNode` advances on a `100 ms` wall timer. +- `EnvNode` publishes `env_data` from `timer_callback()`. +- `EnvNode` stores the latest thrust command in `u_now_` when `actuation_cmd` is called. +- `EnvNode` does not use service-call-driven lock-step stepping. +- `GncNode` computes control from `env_data` and sends thrust using `async_send_request()`. +- `EnvNode` defaults `max_steps` to `91` and `min_subscribers` to `0`. +- `sim.launch.py` currently overrides `min_subscribers` to `1`. +- `test_dlqr_reference_launch.py` overrides `min_subscribers` to `2` so the recorder and + `gnc_node` are both present before the run starts. + +## V1 ROS-Facing Contract + +The bridge must preserve the existing ROS contract for the current DLQR path. + +### Telemetry boundary + +- Topic name: `env_data` +- Message type: `std_msgs/msg/Float64MultiArray` +- Required ordering: `[x, y, z, vx, vy, vz]` +- Required length: exactly `6` + +### Control boundary + +- Service name: `actuation_cmd` +- Service type: `distributed_satellite_sim/srv/ActuationCmd` +- Request shape: `float64[3] thrust` +- Response shape: `bool success` + +### Timing and lifecycle contract + +- The bridge runs a `100 ms` wall timer, matching `EnvNode`. +- The bridge uses latest-command-wins semantics, matching `EnvNode`. +- The service callback stores the newest `thrust[3]` for use on the next timer tick. +- The timer tick advances the backend and publishes the next `env_data` sample. +- The bridge respects `min_subscribers` before the first advance or publish. +- The bridge defaults `max_steps` to `91` and `min_subscribers` to `0` to mirror + `EnvNode` exactly. Launch files may override these defaults for test or operator needs. + +## Role Split + +The bridge package should be split into simulator-agnostic bridge logic and simulator-specific +backend drivers. + +### Simulator-agnostic bridge logic + +The generic bridge node owns: + +- ROS publisher creation for `env_data` +- ROS service creation for `actuation_cmd` +- parameter parsing +- timer scheduling +- startup gating through `min_subscribers` +- step counting and `max_steps` handling +- dimension validation for published state and received control vectors +- error handling and ROS logging + +This logic should not depend on Basilisk-specific types, paths, coordinate conventions, +or scenario setup. + +### Simulator-specific backend drivers + +A backend driver owns: + +- simulator import and initialization +- simulator-native state reads +- translation from simulator-native telemetry into ASTRO's six-state ROS vector +- translation from ASTRO's `thrust[3]` command into simulator-native actuation inputs +- simulator-native timing or stepping calls +- simulator-specific runtime requirements and provenance + +To avoid overloading terms from the manuscript diagrams, the whole ROS-facing component is the +"bridge" or "adapter." Basilisk-specific code should be called a "backend" or +"backend driver." + +## Python and C++ Interop + +The bridge is intended to be implemented in Python using `rclpy`, while `gnc_node` remains in +C++ using `rclcpp`. + +This is an intentional design choice: + +- Basilisk integration is expected to be easier from Python. +- ROS 2 topic and service interoperability is language-agnostic at the DDS boundary. +- `gnc_node` can remain unchanged and continue using the existing `ActuationCmd` service. + +The `external_sim_bridge` package should declare a direct dependency on +`distributed_satellite_sim` so it can import and serve `ActuationCmd`. + +## Deterministic Regression Backend + +Before Basilisk is introduced, the bridge should ship with a deterministic linear-dynamics +backend that mirrors the current `EnvNode` implementation exactly. + +That backend should use: + +- the same initial state +- the same `Ad` matrix +- the same `Bd` matrix +- the same `100 ms` timer cadence at the bridge-node layer +- the same latest-command-wins update semantics + +This backend is not a toy stub. It is the bridge-core regression target that allows: + +- closed-loop testing with the existing `gnc_node` +- comparison against the canonical installed DLQR reference fixture owned by + `distributed_satellite_sim` at + `share/distributed_satellite_sim/test/data/dlqr_reference_trajectory.csv` +- isolation of bridge-architecture bugs before simulator-specific Basilisk work begins + +## Failure Handling + +Failure behavior should distinguish between service-path validation problems and runtime +backend failures. + +### Service-path validation + +If the bridge receives malformed control input or the backend rejects an actuation command +during service-path validation: + +- the bridge should log a clear error +- the bridge should return `success = false` + +### Timer-path runtime failure + +If the backend throws or fails while advancing on the timer tick: + +- the bridge should log the failure clearly +- the failure should be treated as a backend/runtime error, not as a service failure +- the timer should stop so the node fails in a defined and visible way + +## Manuscript Alignment + +Issue `#24` validates the manuscript branch's external deployment pattern, but v1 does not +implement the richer ROS-side decomposition shown in the manuscript's external architecture +figure. + +For v1: + +- the bridge validates the external deployment mode +- the ROS-facing contract stays the same as the current `env_node` to `gnc_node` boundary +- the existing monolithic `gnc_node` remains intact + +This keeps `#24` separate from issue `#6`, which is the broader controller and interface +generalization track. + +## Basilisk Paper Comparison + +The current Basilisk backend matches the manuscript and Basilisk/ROS bridge literature at the +high-level adapter boundary only: + +- Basilisk remains behind a ROS adapter instead of leaking simulator-native messages into the ROS + graph +- `gnc_node` remains unchanged and still consumes `env_data` and calls `actuation_cmd` +- simulator-specific setup, message translation, and timing live inside the Basilisk backend + +For v1, the implementation intentionally does not reproduce a richer paper-style decomposition of +navigation, guidance, and control modules on the ROS side. The bridge validates the external +deployment pattern first while preserving ASTRO's current ROS contract. + +## Correction to Presentation Wording + +Some presentation text describes `gnc_node` as if it "serves" `actuation_cmd`. +That is not how the code behaves on `main`. + +The correct client/server roles are: + +- `gnc_node` is the `actuation_cmd` client +- `env_node`, and later the bridge, is the `actuation_cmd` server + +## Explicit Non-Goals for `#24` + +The following are out of scope for v1 bridge validation: + +- changing `env_data` or `ActuationCmd` schemas +- introducing a generalized telemetry message format +- implementing the manuscript's full Sensor/Nav/Guidance/Control decomposition +- universal controller selection or interface redesign under issue `#6` +- circular-buffer or operator tooling work under issue `#25` +- showcase or presentation packaging work under issue `#26` + +If Basilisk reveals that the current ROS boundary is too narrow, the bridge work should record +that limitation and open follow-on interface issues rather than widening issue `#24`. + +## Basilisk Assumptions Template + +The Basilisk backend implementation should record the following in code comments and user-facing +documentation: + +- Basilisk version or source provenance +- scenario name and entrypoint used for validation +- exact telemetry fields used to build `[x, y, z, vx, vy, vz]` +- frame and axis conventions assumed during mapping +- timing model used between bridge timer ticks and Basilisk stepping +- actuation interface used for translational thrust +- additional runtime dependencies or environment setup requirements +- known gaps, approximations, or validation blockers + +## Current Basilisk Backend Provenance + +The current `basilisk` backend is pinned to the Basilisk Python runtime package +`bsk==2.10.0` for automated validation. + +Documentation and runtime references used while implementing the backend: + +- `SimulationBaseClass` API from Basilisk `2.4.0` +- `thrusterDynamicEffector` API from Basilisk `2.4.0` +- `simIncludeThruster` Python helper from Basilisk `2.4.0` +- `SCStatesMsgPayload` message fields from Basilisk `2.4.0` +- `THRArrayOnTimeCmdMsgPayload` message fields from Basilisk `2.4.0` +- `spacecraft` module API from Basilisk `2.10.0` +- validated CI runtime pin: `bsk==2.10.0` + +## Current Basilisk Scenario Assumptions + +The current backend uses an internal validation scenario named +`translational_3dof_axis_thrusters`. + +Current scenario assumptions are: + +- initial translational state matches ASTRO's current `EnvNode` default state +- telemetry is read from `scStateOutMsg.r_BN_N` and `scStateOutMsg.v_BN_N` +- the exported ROS vector remains `[x, y, z, vx, vy, vz]` +- the spacecraft attitude is initialized to zero and angular dynamics are initialized to zero +- translational control is represented by six center-of-mass thrusters aligned to `+x`, `-x`, + `+y`, `-y`, `+z`, and `-z` +- ROS `thrust[3]` is split into positive and negative axis pairs using one-to-one axis ordering +- each axis command is converted into Basilisk thruster on-time requests over the bridge's + `100 ms` timer period + +## Validated Runtime Record + +The current backend requires: + +- the Basilisk Python package to be installed and importable as `Basilisk` +- the ROS 2 workspace to be built with both `distributed_satellite_sim` and + `external_sim_bridge` +- launch via `external_sim_bridge/launch/bridge_sim.launch.py` or direct `ros2 run` + +Authoritative validation environment: + +- GitHub Actions workflow `.github/workflows/validate-external-sim-bridge.yml` +- base runtime `ghcr.io/accommodus/astro:latest` +- Basilisk runtime pin `bsk==2.10.0` +- CI guard `ASTRO_EXPECT_BASILISK=1` so Basilisk tests fail if the runtime is absent + +Live validation performed in the authoritative environment: + +- direct Basilisk backend smoke coverage in `test/test_basilisk_backend.py` +- end-to-end bridge launch coverage in `test/test_bridge_basilisk_launch.py` +- fake-backend parity coverage against the installed canonical `distributed_satellite_sim` + trajectory fixture in `test/test_bridge_reference_launch.py` + +Remaining limitations: + +- the Basilisk validation scenario is still the fixed internal + `translational_3dof_axis_thrusters` bridge scenario, not a full HCW-equivalent model +- local development environments that do not install Basilisk will skip the Basilisk-specific + tests by default diff --git a/src/external_sim_bridge/external_sim_bridge/__init__.py b/src/external_sim_bridge/external_sim_bridge/__init__.py new file mode 100644 index 0000000..a6157ab --- /dev/null +++ b/src/external_sim_bridge/external_sim_bridge/__init__.py @@ -0,0 +1 @@ +"""External simulator bridge package.""" diff --git a/src/external_sim_bridge/external_sim_bridge/backends/__init__.py b/src/external_sim_bridge/external_sim_bridge/backends/__init__.py new file mode 100644 index 0000000..a61b533 --- /dev/null +++ b/src/external_sim_bridge/external_sim_bridge/backends/__init__.py @@ -0,0 +1,16 @@ +from external_sim_bridge.backends.base import ExternalSimBackend +from external_sim_bridge.backends.basilisk_backend import BasiliskBackend +from external_sim_bridge.backends.fake_backend import FakeBackend + + +def create_backend(backend_type: str) -> ExternalSimBackend: + normalized_backend_type = backend_type.strip().lower() + + if normalized_backend_type == 'fake': + return FakeBackend() + if normalized_backend_type == 'basilisk': + return BasiliskBackend() + + raise ValueError( + f"Unsupported backend_type '{backend_type}'. Available backends: ['fake', 'basilisk']" + ) diff --git a/src/external_sim_bridge/external_sim_bridge/backends/base.py b/src/external_sim_bridge/external_sim_bridge/backends/base.py new file mode 100644 index 0000000..e272659 --- /dev/null +++ b/src/external_sim_bridge/external_sim_bridge/backends/base.py @@ -0,0 +1,23 @@ +from __future__ import annotations + +from typing import Protocol, Sequence + + +class BackendValidationError(ValueError): + """Raised when backend-facing state or control validation fails.""" + + +class ExternalSimBackend(Protocol): + """Simulator-specific backend contract used by the generic bridge node.""" + + def initialize(self) -> None: + """Initialize any simulator-side state before the bridge starts.""" + + def validate_control(self, control: Sequence[float]) -> None: + """Validate a control vector before the bridge stores it.""" + + def apply_control(self, control: Sequence[float]) -> None: + """Apply the latest validated control command before advancing.""" + + def advance(self) -> Sequence[float]: + """Advance the backend once and return `[x, y, z, vx, vy, vz]`.""" diff --git a/src/external_sim_bridge/external_sim_bridge/backends/basilisk_backend.py b/src/external_sim_bridge/external_sim_bridge/backends/basilisk_backend.py new file mode 100644 index 0000000..344a5ba --- /dev/null +++ b/src/external_sim_bridge/external_sim_bridge/backends/basilisk_backend.py @@ -0,0 +1,209 @@ +from __future__ import annotations + +from typing import Any, Sequence + +from external_sim_bridge.backends.base import BackendValidationError + + +class BasiliskBackend: + """Basilisk backend for a fixed translational validation scenario. + + Provenance assumptions: + - designed against Basilisk's documented Python APIs for `SimulationBaseClass`, + `spacecraft.Spacecraft`, `thrusterDynamicEffector`, `simIncludeThruster`, + `SCStatesMsgPayload`, and `THRArrayOnTimeCmdMsgPayload` + - the validation scenario is an internal bridge-only configuration named + `translational_3dof_axis_thrusters` + + Scenario assumptions: + - telemetry uses `scStateOutMsg.r_BN_N` and `scStateOutMsg.v_BN_N` + - the exported ROS state is `[x, y, z, vx, vy, vz]` in the same inertial frame + - the spacecraft attitude is initialized to zero and no torque is introduced, so + body-frame thruster axes stay aligned with the inertial axes for this scenario + - translational actuation is modeled with six center-of-mass thrusters: + `+x`, `-x`, `+y`, `-y`, `+z`, `-z` + - each ROS thrust component maps to the matching Basilisk axis pair using + one-to-one axis ordering and a fixed force-to-on-time conversion over the + bridge's 100 ms step + + Known limitation: + - this scenario is a bridge-validation path, not an HCW-equivalent Basilisk model + """ + + _STEP_PERIOD_SEC = 0.1 + _TASK_NAME = 'basilisk_bridge_task' + _PROCESS_NAME = 'basilisk_bridge_process' + _SPACECRAFT_TAG = 'basilisk_bridge_spacecraft' + _THRUSTER_TAG = 'basilisk_bridge_thrusters' + _AXIS_THRUSTER_MAX_THRUST_N = 1.0 + _INITIAL_POSITION_M = [20.0, 20.0, 20.0] + _INITIAL_VELOCITY_MPS = [0.00930458, -0.0467472, 0.00798343] + _THRUSTER_LAYOUT = ( + ('+x', [1.0, 0.0, 0.0]), + ('-x', [-1.0, 0.0, 0.0]), + ('+y', [0.0, 1.0, 0.0]), + ('-y', [0.0, -1.0, 0.0]), + ('+z', [0.0, 0.0, 1.0]), + ('-z', [0.0, 0.0, -1.0]), + ) + + def __init__(self) -> None: + self._control = [0.0, 0.0, 0.0] + self._current_stop_time_ns = 0 + self._step_period_ns: int | None = None + self._messaging: Any = None + self._sim: Any = None + self._macros: Any = None + self._spacecraft: Any = None + self._thruster_effector: Any = None + self._thruster_command_msg: Any = None + + def initialize(self) -> None: + modules = self._import_basilisk_modules() + self._messaging = modules['messaging'] + self._macros = modules['macros'] + self._sim = modules['SimulationBaseClass'].SimBaseClass() + self._step_period_ns = self._macros.sec2nano(self._STEP_PERIOD_SEC) + + dynamics_process = self._sim.CreateNewProcess(self._PROCESS_NAME) + dynamics_process.addTask( + self._sim.CreateNewTask(self._TASK_NAME, self._step_period_ns) + ) + + self._spacecraft = self._create_spacecraft(modules['spacecraft']) + self._thruster_effector = self._create_thrusters( + sim_include_thruster=modules['simIncludeThruster'], + thruster_dynamic_effector=modules['thrusterDynamicEffector'], + ) + + self._sim.AddModelToTask(self._TASK_NAME, self._thruster_effector) + self._sim.AddModelToTask(self._TASK_NAME, self._spacecraft) + + zero_cmd = self._build_thruster_payload([0.0] * len(self._THRUSTER_LAYOUT)) + self._thruster_command_msg = self._messaging.THRArrayOnTimeCmdMsg().write(zero_cmd) + self._thruster_effector.cmdsInMsg.subscribeTo(self._thruster_command_msg) + + self._sim.InitializeSimulation() + + def validate_control(self, control: Sequence[float]) -> None: + if len(control) != 3: + raise BackendValidationError( + f'Basilisk backend expected a 3-element control vector, received {len(control)}' + ) + + for axis_name, value in zip(('x', 'y', 'z'), control): + if abs(float(value)) > self._AXIS_THRUSTER_MAX_THRUST_N: + raise BackendValidationError( + f'Basilisk {axis_name}-axis command {value} exceeds the configured ' + f'per-axis thrust limit of {self._AXIS_THRUSTER_MAX_THRUST_N} N' + ) + + def apply_control(self, control: Sequence[float]) -> None: + self.validate_control(control) + self._control = [float(value) for value in control] + # Basilisk treats later writes with the default timestamp of 0 ns as stale once the + # simulation has advanced. Stamp each command at the current simulation time so + # post-start actuation updates are consumed on the next integration step. + self._thruster_command_msg.write( + self._build_thruster_payload(self._control_to_on_times()), + self._current_stop_time_ns, + ) + + def advance(self) -> list[float]: + if self._step_period_ns is None: + raise RuntimeError('Basilisk backend was not initialized before advance()') + + self._current_stop_time_ns += self._step_period_ns + self._sim.ConfigureStopTime(self._current_stop_time_ns) + self._sim.ExecuteSimulation() + + state = self._spacecraft.scStateOutMsg.read() + return [ + float(state.r_BN_N[0]), + float(state.r_BN_N[1]), + float(state.r_BN_N[2]), + float(state.v_BN_N[0]), + float(state.v_BN_N[1]), + float(state.v_BN_N[2]), + ] + + def _import_basilisk_modules(self) -> dict[str, Any]: + try: + from Basilisk.architecture import messaging + from Basilisk.simulation import spacecraft, thrusterDynamicEffector + from Basilisk.utilities import SimulationBaseClass, macros, simIncludeThruster + except ModuleNotFoundError as error: + raise ImportError( + 'Basilisk backend selected, but the Basilisk Python package is not installed ' + 'or is missing required modules' + ) from error + + return { + 'SimulationBaseClass': SimulationBaseClass, + 'macros': macros, + 'messaging': messaging, + 'simIncludeThruster': simIncludeThruster, + 'spacecraft': spacecraft, + 'thrusterDynamicEffector': thrusterDynamicEffector, + } + + def _create_spacecraft(self, spacecraft_module: Any) -> Any: + spacecraft_object = spacecraft_module.Spacecraft() + spacecraft_object.ModelTag = self._SPACECRAFT_TAG + spacecraft_object.hub.mHub = 1.0 + spacecraft_object.hub.r_BcB_B = [0.0, 0.0, 0.0] + spacecraft_object.hub.IHubPntBc_B = [ + [1.0, 0.0, 0.0], + [0.0, 1.0, 0.0], + [0.0, 0.0, 1.0], + ] + spacecraft_object.hub.r_CN_NInit = self._INITIAL_POSITION_M + spacecraft_object.hub.v_CN_NInit = self._INITIAL_VELOCITY_MPS + spacecraft_object.hub.sigma_BNInit = [0.0, 0.0, 0.0] + spacecraft_object.hub.omega_BN_BInit = [0.0, 0.0, 0.0] + return spacecraft_object + + def _create_thrusters( + self, + sim_include_thruster: Any, + thruster_dynamic_effector: Any, + ) -> Any: + thruster_effector = thruster_dynamic_effector.ThrusterDynamicEffector() + thruster_factory = sim_include_thruster.thrusterFactory() + + for label, axis in self._THRUSTER_LAYOUT: + thruster_factory.create( + 'Blank_Thruster', + [0.0, 0.0, 0.0], + axis, + label=label, + MaxThrust=float(self._AXIS_THRUSTER_MAX_THRUST_N), + useMinPulseTime=False, + ) + + thruster_factory.addToSpacecraft( + self._THRUSTER_TAG, + thruster_effector, + self._spacecraft, + ) + return thruster_effector + + def _control_to_on_times(self) -> list[float]: + x_force, y_force, z_force = self._control + scale = self._STEP_PERIOD_SEC / self._AXIS_THRUSTER_MAX_THRUST_N + return [ + max(x_force, 0.0) * scale, + max(-x_force, 0.0) * scale, + max(y_force, 0.0) * scale, + max(-y_force, 0.0) * scale, + max(z_force, 0.0) * scale, + max(-z_force, 0.0) * scale, + ] + + def _build_thruster_payload(self, on_times: Sequence[float]) -> Any: + payload = self._messaging.THRArrayOnTimeCmdMsgPayload() + requested_on_times = list(payload.OnTimeRequest) + for index, on_time in enumerate(on_times): + requested_on_times[index] = float(on_time) + payload.OnTimeRequest = requested_on_times + return payload diff --git a/src/external_sim_bridge/external_sim_bridge/backends/fake_backend.py b/src/external_sim_bridge/external_sim_bridge/backends/fake_backend.py new file mode 100644 index 0000000..ea3acd7 --- /dev/null +++ b/src/external_sim_bridge/external_sim_bridge/backends/fake_backend.py @@ -0,0 +1,57 @@ +from __future__ import annotations + +from typing import Sequence + +import numpy as np + +from external_sim_bridge.backends.base import BackendValidationError + + +class FakeBackend: + """Deterministic linear backend that mirrors the current env-node dynamics.""" + + def __init__(self) -> None: + self._state = np.array( + [20.0, 20.0, 20.0, 0.00930458, -0.0467472, 0.00798343], + dtype=float, + ) + self._control = np.zeros(3, dtype=float) + self._ad = np.array( + [ + [1.25645279151274, 0.0, 0.0, 349.682205848334, 147.7805051, 0.0], + [-0.0716204647063059, 1.0, 0.0, -147.7805051, 318.728823393334, 0.0], + [0.0, 0.0, 0.914515736162421, 0.0, 0.0, 349.682205848334], + [0.0014040831838806, 0.0, 0.0, 0.914515736162421, 0.809100657054006, 0.0], + [-0.00059338484671504, 0.0, 0.0, -0.809100657054006, 0.658062944649682, 0.0], + [0.0, 0.0, -0.000468027727960198, 0.0, 0.0, 0.914515736162421], + ], + dtype=float, + ) + self._bd = np.array( + [ + [63868.7072544296, 17836.8364281426, 0.0], + [-17836.8364281426, 61074.8290177183, 0.0], + [0.0, 0.0, 63868.7072544296], + [349.682205848334, 147.7805051, 0.0], + [-147.7805051, 318.728823393334, 0.0], + [0.0, 0.0, 349.682205848334], + ], + dtype=float, + ) + + def initialize(self) -> None: + return None + + def validate_control(self, control: Sequence[float]) -> None: + if len(control) != 3: + raise BackendValidationError( + f'Fake backend expected a 3-element control vector, received {len(control)}' + ) + + def apply_control(self, control: Sequence[float]) -> None: + self.validate_control(control) + self._control = np.asarray(control, dtype=float) + + def advance(self) -> list[float]: + self._state = self._ad @ self._state + self._bd @ self._control + return self._state.astype(float).tolist() diff --git a/src/external_sim_bridge/external_sim_bridge/bridge_node.py b/src/external_sim_bridge/external_sim_bridge/bridge_node.py new file mode 100644 index 0000000..c6f7d6e --- /dev/null +++ b/src/external_sim_bridge/external_sim_bridge/bridge_node.py @@ -0,0 +1,147 @@ +from __future__ import annotations + +import math +from typing import Sequence + +from distributed_satellite_sim.srv import ActuationCmd +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float64MultiArray + +from external_sim_bridge.backends import create_backend +from external_sim_bridge.backends.base import BackendValidationError, ExternalSimBackend +from external_sim_bridge.config import BridgeConfig, load_bridge_config + + +class ExternalSimBridgeNode(Node): + def __init__(self) -> None: + super().__init__('external_sim_bridge') + + try: + self._config = load_bridge_config(self) + self._backend = self._create_backend(self._config) + except Exception as error: + self.get_logger().error(f'Bridge startup failed: {error}') + raise + + self._latest_command = [0.0, 0.0, 0.0] + self._step = 0 + self._sim_started = False + self._runtime_failure: Exception | None = None + + self._state_pub = self.create_publisher(Float64MultiArray, 'env_data', 10) + self._cmd_srv = self.create_service( + ActuationCmd, + 'actuation_cmd', + self._handle_actuation_command, + ) + self._timer = self.create_timer( + self._config.timer_period_sec, + self._handle_timer_tick, + ) + + self.get_logger().info( + 'External simulator bridge started ' + f'(backend_type={self._config.backend_type}, max_steps={self._config.max_steps}, ' + f'min_subscribers={self._config.min_subscribers})' + ) + + def _create_backend(self, config: BridgeConfig) -> ExternalSimBackend: + backend = create_backend(config.backend_type) + backend.initialize() + return backend + + def _handle_actuation_command( + self, + request: ActuationCmd.Request, + response: ActuationCmd.Response, + ) -> ActuationCmd.Response: + try: + control = self._validate_vector( + values=request.thrust, + expected_length=3, + vector_name='control', + ) + self._backend.validate_control(control) + except (BackendValidationError, ValueError) as error: + self.get_logger().error(f'Invalid actuation command: {error}') + response.success = False + return response + + self._latest_command = control + response.success = True + self.get_logger().info( + 'actuation_cmd received: u = [%.6e, %.6e, %.6e]' + % tuple(self._latest_command) + ) + return response + + def _handle_timer_tick(self) -> None: + if not self._sim_started: + if self._state_pub.get_subscription_count() < self._config.min_subscribers: + return + self._sim_started = True + + if self._config.max_steps > 0 and self._step >= self._config.max_steps: + self._timer.cancel() + self.get_logger().info(f'Simulation complete after {self._step} steps') + return + + try: + self._backend.apply_control(self._latest_command) + state = self._validate_vector( + values=self._backend.advance(), + expected_length=6, + vector_name='state', + ) + except Exception as error: + self.get_logger().error(f'Backend runtime failure on step {self._step}: {error}') + self._timer.cancel() + self._runtime_failure = RuntimeError( + f'Backend runtime failure on step {self._step}: {error}' + ) + rclpy.shutdown() + return + + self._state_pub.publish(Float64MultiArray(data=state)) + self.get_logger().info( + 'step %d: x = [%.6f, %.6f, %.6f, %.6f, %.6f, %.6f]' + % (self._step, *state) + ) + self._step += 1 + + def _validate_vector( + self, + values: Sequence[float], + expected_length: int, + vector_name: str, + ) -> list[float]: + vector = list(values) + if len(vector) != expected_length: + raise ValueError( + f'{vector_name} vector must have length {expected_length}, received {len(vector)}' + ) + + validated = [] + for index, value in enumerate(vector): + numeric_value = float(value) + if not math.isfinite(numeric_value): + raise ValueError( + f'{vector_name}[{index}] must be finite, received {numeric_value!r}' + ) + validated.append(numeric_value) + + return validated + + +def spin_bridge() -> None: + bridge = ExternalSimBridgeNode() + runtime_failure = None + try: + rclpy.spin(bridge) + finally: + runtime_failure = bridge._runtime_failure + bridge.destroy_node() + + if runtime_failure is not None: + raise runtime_failure diff --git a/src/external_sim_bridge/external_sim_bridge/config.py b/src/external_sim_bridge/external_sim_bridge/config.py new file mode 100644 index 0000000..fe4ac6d --- /dev/null +++ b/src/external_sim_bridge/external_sim_bridge/config.py @@ -0,0 +1,30 @@ +from dataclasses import dataclass + +from rclpy.node import Node + + +@dataclass(frozen=True) +class BridgeConfig: + backend_type: str + max_steps: int + min_subscribers: int + timer_period_sec: float = 0.1 + + +def load_bridge_config(node: Node) -> BridgeConfig: + node.declare_parameter('backend_type', 'fake') + node.declare_parameter('max_steps', 91) + node.declare_parameter('min_subscribers', 0) + + config = BridgeConfig( + backend_type=node.get_parameter('backend_type').get_parameter_value().string_value, + max_steps=node.get_parameter('max_steps').get_parameter_value().integer_value, + min_subscribers=node.get_parameter('min_subscribers').get_parameter_value().integer_value, + ) + + if config.max_steps < 0: + raise ValueError('max_steps must be greater than or equal to 0') + if config.min_subscribers < 0: + raise ValueError('min_subscribers must be greater than or equal to 0') + + return config diff --git a/src/external_sim_bridge/external_sim_bridge/main.py b/src/external_sim_bridge/external_sim_bridge/main.py new file mode 100644 index 0000000..a30db5d --- /dev/null +++ b/src/external_sim_bridge/external_sim_bridge/main.py @@ -0,0 +1,14 @@ +import rclpy + +from external_sim_bridge.bridge_node import spin_bridge + + +def main(args=None) -> None: + rclpy.init(args=args) + try: + spin_bridge() + except KeyboardInterrupt: + pass + finally: + if rclpy.ok(): + rclpy.shutdown() diff --git a/src/external_sim_bridge/launch/bridge_sim.launch.py b/src/external_sim_bridge/launch/bridge_sim.launch.py new file mode 100644 index 0000000..f82019b --- /dev/null +++ b/src/external_sim_bridge/launch/bridge_sim.launch.py @@ -0,0 +1,67 @@ +#!/usr/bin/env python3 + +# Copyright 2026 ASTRO +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + backend_type_arg = DeclareLaunchArgument( + 'backend_type', + default_value='basilisk', + description='Bridge backend to run (default: basilisk)', + ) + + max_steps_arg = DeclareLaunchArgument( + 'max_steps', + default_value='91', + description='Number of bridge steps to execute (0 = infinite)', + ) + + min_subscribers_arg = DeclareLaunchArgument( + 'min_subscribers', + default_value='1', + description='Minimum number of env_data subscribers before advancing', + ) + + bridge_node = Node( + package='external_sim_bridge', + executable='bridge_node', + name='external_sim_bridge', + output='screen', + parameters=[{ + 'backend_type': LaunchConfiguration('backend_type'), + 'max_steps': LaunchConfiguration('max_steps'), + 'min_subscribers': LaunchConfiguration('min_subscribers'), + }], + ) + + gnc_node = Node( + package='distributed_satellite_sim', + executable='gnc_node', + name='gnc_node', + output='screen', + ) + + return LaunchDescription([ + backend_type_arg, + max_steps_arg, + min_subscribers_arg, + bridge_node, + gnc_node, + ]) diff --git a/src/external_sim_bridge/package.xml b/src/external_sim_bridge/package.xml new file mode 100644 index 0000000..44e6e2d --- /dev/null +++ b/src/external_sim_bridge/package.xml @@ -0,0 +1,31 @@ + + + + external_sim_bridge + 0.1.0 + + Generic ROS 2 bridge package for external simulator validation. + + Dylan Long + Apache-2.0 + + distributed_satellite_sim + rclpy + std_msgs + launch + launch_ros + python3-numpy + + ament_lint_auto + ament_lint_common + ament_index_python + launch_ros + launch_testing + python3-pytest + + + ament_python + + diff --git a/src/external_sim_bridge/pytest.ini b/src/external_sim_bridge/pytest.ini new file mode 100644 index 0000000..2266419 --- /dev/null +++ b/src/external_sim_bridge/pytest.ini @@ -0,0 +1,2 @@ +[pytest] +testpaths = test diff --git a/src/external_sim_bridge/resource/external_sim_bridge b/src/external_sim_bridge/resource/external_sim_bridge new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/src/external_sim_bridge/resource/external_sim_bridge @@ -0,0 +1 @@ + diff --git a/src/external_sim_bridge/setup.cfg b/src/external_sim_bridge/setup.cfg new file mode 100644 index 0000000..be7157f --- /dev/null +++ b/src/external_sim_bridge/setup.cfg @@ -0,0 +1,5 @@ +[develop] +script_dir=$base/lib/external_sim_bridge + +[install] +install_scripts=$base/lib/external_sim_bridge diff --git a/src/external_sim_bridge/setup.py b/src/external_sim_bridge/setup.py new file mode 100644 index 0000000..7f0f69b --- /dev/null +++ b/src/external_sim_bridge/setup.py @@ -0,0 +1,39 @@ +from setuptools import find_packages, setup + + +package_name = 'external_sim_bridge' + + +setup( + name=package_name, + version='0.1.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', [f'resource/{package_name}']), + (f'share/{package_name}', ['package.xml']), + (f'share/{package_name}/doc', ['doc/design_note.md']), + (f'share/{package_name}/launch', ['launch/bridge_sim.launch.py']), + ( + f'share/{package_name}/test', + [ + 'test/test_fake_backend.py', + 'test/test_bridge_node.py', + 'test/test_bridge_reference_launch.py', + 'test/test_basilisk_backend.py', + 'test/test_bridge_basilisk_launch.py', + ], + ), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Dylan Long', + maintainer_email='dylan.long@ufl.edu', + description='Generic ROS 2 bridge package for external simulator validation.', + license='Apache License 2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'bridge_node = external_sim_bridge.main:main', + ], + }, +) diff --git a/src/external_sim_bridge/test/__init__.py b/src/external_sim_bridge/test/__init__.py new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/src/external_sim_bridge/test/__init__.py @@ -0,0 +1 @@ + diff --git a/src/external_sim_bridge/test/test_basilisk_backend.py b/src/external_sim_bridge/test/test_basilisk_backend.py new file mode 100644 index 0000000..58e69f5 --- /dev/null +++ b/src/external_sim_bridge/test/test_basilisk_backend.py @@ -0,0 +1,56 @@ +import math +import os +import unittest + +from external_sim_bridge.backends.basilisk_backend import BasiliskBackend + + +EXPECT_BASILISK = os.environ.get('ASTRO_EXPECT_BASILISK') == '1' + +try: + import Basilisk # noqa: F401 +except ModuleNotFoundError as error: + BASILISK_AVAILABLE = False + BASILISK_IMPORT_ERROR = error +else: + BASILISK_AVAILABLE = True + BASILISK_IMPORT_ERROR = None + + +if EXPECT_BASILISK and not BASILISK_AVAILABLE: + raise AssertionError( + 'ASTRO_EXPECT_BASILISK=1 but the Basilisk runtime is not importable: ' + f'{BASILISK_IMPORT_ERROR}' + ) + + +@unittest.skipUnless(BASILISK_AVAILABLE, 'Basilisk runtime is not installed') +class BasiliskBackendSmokeTest(unittest.TestCase): + def _assert_state_vector(self, state) -> None: + self.assertEqual(len(state), 6) + for value in state: + self.assertTrue(math.isfinite(float(value))) + + def _rollout(self, control, steps: int = 3): + backend = BasiliskBackend() + backend.initialize() + backend.apply_control(control) + + state = None + for _ in range(steps): + state = backend.advance() + + self.assertIsNotNone(state) + self._assert_state_vector(state) + return state + + def test_zero_control_rollout_returns_finite_state(self): + state = self._rollout([0.0, 0.0, 0.0]) + self.assertGreater(state[0], 0.0) + + def test_positive_x_control_increases_x_position_and_velocity(self): + zero_state = self._rollout([0.0, 0.0, 0.0]) + positive_x_state = self._rollout([0.5, 0.0, 0.0]) + + self.assertGreater(positive_x_state[0], zero_state[0]) + self.assertGreater(positive_x_state[3], zero_state[3]) diff --git a/src/external_sim_bridge/test/test_bridge_basilisk_launch.py b/src/external_sim_bridge/test/test_bridge_basilisk_launch.py new file mode 100644 index 0000000..857068e --- /dev/null +++ b/src/external_sim_bridge/test/test_bridge_basilisk_launch.py @@ -0,0 +1,197 @@ +#!/usr/bin/env python3 + +import math +import os +import time +import unittest + +from ament_index_python.packages import get_package_share_directory +import launch +from launch.actions import EmitEvent +from launch.actions import IncludeLaunchDescription +from launch.actions import TimerAction +from launch.events import Shutdown +from launch.launch_description_sources import PythonLaunchDescriptionSource +import launch_testing +import launch_testing.actions +import launch_testing.asserts +import pytest +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile +from std_msgs.msg import Float64MultiArray + + +EXPECT_BASILISK = os.environ.get('ASTRO_EXPECT_BASILISK') == '1' + +try: + import Basilisk # noqa: F401 +except ModuleNotFoundError as error: + BASILISK_AVAILABLE = False + BASILISK_IMPORT_ERROR = error +else: + BASILISK_AVAILABLE = True + BASILISK_IMPORT_ERROR = None + + +if EXPECT_BASILISK and not BASILISK_AVAILABLE: + raise AssertionError( + 'ASTRO_EXPECT_BASILISK=1 but the Basilisk runtime is not importable: ' + f'{BASILISK_IMPORT_ERROR}' + ) + + +EXPECTED_STEPS = 5 +STATE_SIZE = 6 +SHUTDOWN_AFTER_SEC = 15.0 +RECEIVE_TIMEOUT_SEC = 12.0 +DISCOVERY_TIMEOUT_SEC = 10.0 +CONTROL_EFFECT_TOLERANCE = 1e-6 + + +class BasiliskTrajectoryRecorder(Node): + def __init__(self): + super().__init__('basilisk_bridge_trajectory_recorder') + self.trajectory = [] + self.subscription = self.create_subscription( + Float64MultiArray, + 'env_data', + self._callback, + QoSProfile(depth=100), + ) + + def _callback(self, msg): + self.trajectory.append(list(msg.data)) + + +if BASILISK_AVAILABLE: + from external_sim_bridge.backends.basilisk_backend import BasiliskBackend + + @pytest.mark.rostest + def generate_test_description(): + package_share = get_package_share_directory('external_sim_bridge') + launch_path = os.path.join(package_share, 'launch', 'bridge_sim.launch.py') + + bridge_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(launch_path), + launch_arguments={ + 'backend_type': 'basilisk', + 'max_steps': str(EXPECTED_STEPS), + 'min_subscribers': '2', + }.items(), + ) + + return ( + launch.LaunchDescription([ + launch_testing.actions.ReadyToTest(), + bridge_launch, + TimerAction( + period=SHUTDOWN_AFTER_SEC, + actions=[EmitEvent(event=Shutdown(reason='basilisk bridge validation complete'))], + ), + ]), + {}, + ) + + + class TestBasiliskBridgeLaunch(unittest.TestCase): + @classmethod + def setUpClass(cls): + if not rclpy.ok(): + rclpy.init() + cls.recorder = BasiliskTrajectoryRecorder() + cls.zero_control_trajectory = cls._build_zero_control_trajectory() + + @classmethod + def tearDownClass(cls): + cls.recorder.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + @staticmethod + def _build_zero_control_trajectory(): + backend = BasiliskBackend() + backend.initialize() + backend.apply_control([0.0, 0.0, 0.0]) + + trajectory = [] + for _ in range(EXPECTED_STEPS): + trajectory.append(backend.advance()) + return trajectory + + def test_basilisk_bridge_closed_loop_affects_state_trajectory(self, proc_output): + discovery_deadline = time.monotonic() + DISCOVERY_TIMEOUT_SEC + while ( + self.recorder.count_publishers('env_data') == 0 + and time.monotonic() < discovery_deadline + ): + rclpy.spin_once(self.recorder, timeout_sec=0.1) + + self.assertGreater( + self.recorder.count_publishers('env_data'), + 0, + 'env_data publisher was not discovered within timeout', + ) + + deadline = time.monotonic() + RECEIVE_TIMEOUT_SEC + while ( + len(self.recorder.trajectory) < EXPECTED_STEPS + and time.monotonic() < deadline + ): + rclpy.spin_once(self.recorder, timeout_sec=0.1) + + received = len(self.recorder.trajectory) + self.assertEqual( + received, + EXPECTED_STEPS, + f'Expected {EXPECTED_STEPS} env_data messages, received {received}', + ) + + proc_output.assertWaitFor( + 'actuation_cmd received', + timeout=DISCOVERY_TIMEOUT_SEC, + ) + + saw_motion = False + saw_closed_loop_divergence = False + previous_state = None + for step, (state, baseline_state) in enumerate( + zip(self.recorder.trajectory, self.zero_control_trajectory) + ): + self.assertEqual( + len(state), + STATE_SIZE, + f'Trajectory row {step} should contain {STATE_SIZE} state values', + ) + for index, value in enumerate(state): + self.assertTrue( + math.isfinite(float(value)), + f'Trajectory row {step} state[{index}] must be finite', + ) + + if step > 0 and any( + abs(actual - baseline) > CONTROL_EFFECT_TOLERANCE + for actual, baseline in zip(state, baseline_state) + ): + saw_closed_loop_divergence = True + + if previous_state is not None and any( + abs(current - previous) > 1e-9 + for current, previous in zip(state, previous_state) + ): + saw_motion = True + + previous_state = state + + self.assertTrue(saw_motion, 'Basilisk launch trajectory did not change between steps') + self.assertTrue( + saw_closed_loop_divergence, + 'Launched Basilisk trajectory never diverged from the zero-control baseline; ' + 'the actuation path may not have affected the simulator state', + ) + + + @launch_testing.post_shutdown_test() + class TestBasiliskBridgeShutdown(unittest.TestCase): + def test_all_processes_exit_zero(self, proc_info): + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/src/external_sim_bridge/test/test_bridge_node.py b/src/external_sim_bridge/test/test_bridge_node.py new file mode 100644 index 0000000..648150e --- /dev/null +++ b/src/external_sim_bridge/test/test_bridge_node.py @@ -0,0 +1,158 @@ +import time +import unittest +from unittest.mock import patch + +import rclpy +from distributed_satellite_sim.srv import ActuationCmd +from rclpy.executors import SingleThreadedExecutor +from rclpy.node import Node +from std_msgs.msg import Float64MultiArray + +from external_sim_bridge.bridge_node import ExternalSimBridgeNode +from external_sim_bridge.config import BridgeConfig + + +class ExplodingBackend: + def initialize(self) -> None: + return None + + def validate_control(self, control) -> None: + return None + + def apply_control(self, control) -> None: + return None + + def advance(self): + raise RuntimeError('boom') + + +class WrongStateBackend: + def initialize(self) -> None: + return None + + def validate_control(self, control) -> None: + return None + + def apply_control(self, control) -> None: + return None + + def advance(self): + return [1.0, 2.0] + + +class RejectingControlBackend: + def initialize(self) -> None: + return None + + def validate_control(self, control) -> None: + raise ValueError('backend rejected control') + + def apply_control(self, control) -> None: + return None + + def advance(self): + return [0.0] * 6 + + +class BridgeNodeTest(unittest.TestCase): + @classmethod + def setUpClass(cls): + if not rclpy.ok(): + rclpy.init() + + @classmethod + def tearDownClass(cls): + if rclpy.ok(): + rclpy.shutdown() + + def setUp(self): + self.helper_node = Node('external_sim_bridge_test_helper') + self.executor = SingleThreadedExecutor() + self.received_messages = [] + self.subscription = self.helper_node.create_subscription( + Float64MultiArray, + 'env_data', + self.received_messages.append, + 10, + ) + self.client = self.helper_node.create_client(ActuationCmd, 'actuation_cmd') + self.bridge = None + + def tearDown(self): + if self.bridge is not None: + self.executor.remove_node(self.bridge) + self.bridge.destroy_node() + self.bridge = None + self.executor.remove_node(self.helper_node) + self.executor.shutdown() + self.helper_node.destroy_node() + + def _make_bridge(self, *, backend, config: BridgeConfig): + with ( + patch('external_sim_bridge.bridge_node.load_bridge_config', return_value=config), + patch('external_sim_bridge.bridge_node.create_backend', return_value=backend), + ): + self.bridge = ExternalSimBridgeNode() + self.executor.add_node(self.bridge) + self.executor.add_node(self.helper_node) + + def _spin_for(self, seconds: float): + deadline = time.monotonic() + seconds + while time.monotonic() < deadline: + self.executor.spin_once(timeout_sec=0.05) + + def _call_actuation(self, thrust): + self.assertTrue(self.client.wait_for_service(timeout_sec=2.0)) + request = ActuationCmd.Request() + request.thrust = list(thrust) + future = self.client.call_async(request) + deadline = time.monotonic() + 2.0 + while not future.done() and time.monotonic() < deadline: + self.executor.spin_once(timeout_sec=0.05) + self.assertTrue(future.done(), 'actuation_cmd service call did not complete') + return future.result() + + def test_bridge_waits_for_min_subscribers_before_publishing(self): + self._make_bridge( + backend=WrongStateBackend(), + config=BridgeConfig(backend_type='fake', max_steps=91, min_subscribers=2), + ) + + self._spin_for(0.35) + + self.assertEqual(len(self.received_messages), 0) + self.assertIsNone(self.bridge._runtime_failure) + + def test_bridge_returns_failure_when_backend_rejects_control(self): + self._make_bridge( + backend=RejectingControlBackend(), + config=BridgeConfig(backend_type='fake', max_steps=91, min_subscribers=0), + ) + + response = self._call_actuation([1.0, 2.0, 3.0]) + + self.assertFalse(response.success) + + def test_bridge_stops_timer_on_backend_runtime_failure(self): + with patch('external_sim_bridge.bridge_node.rclpy.shutdown', return_value=None): + self._make_bridge( + backend=ExplodingBackend(), + config=BridgeConfig(backend_type='fake', max_steps=91, min_subscribers=0), + ) + + self._spin_for(0.25) + + self.assertIsNotNone(self.bridge._runtime_failure) + self.assertTrue(self.bridge._timer.is_canceled()) + + def test_bridge_rejects_wrong_state_dimension(self): + with patch('external_sim_bridge.bridge_node.rclpy.shutdown', return_value=None): + self._make_bridge( + backend=WrongStateBackend(), + config=BridgeConfig(backend_type='fake', max_steps=91, min_subscribers=0), + ) + + self._spin_for(0.25) + + self.assertIsNotNone(self.bridge._runtime_failure) + self.assertTrue(self.bridge._timer.is_canceled()) diff --git a/src/external_sim_bridge/test/test_bridge_reference_launch.py b/src/external_sim_bridge/test/test_bridge_reference_launch.py new file mode 100644 index 0000000..da830ea --- /dev/null +++ b/src/external_sim_bridge/test/test_bridge_reference_launch.py @@ -0,0 +1,167 @@ +#!/usr/bin/env python3 + +import csv +import os +import time +import unittest + +from ament_index_python.packages import get_package_share_directory +import launch +from launch.actions import EmitEvent +from launch.actions import IncludeLaunchDescription +from launch.actions import TimerAction +from launch.events import Shutdown +from launch.launch_description_sources import PythonLaunchDescriptionSource +import launch_testing +import launch_testing.actions +import launch_testing.asserts +import pytest +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile +from std_msgs.msg import Float64MultiArray + +REFERENCE_FIXTURE_PATH = os.path.join( + get_package_share_directory('distributed_satellite_sim'), + 'test', + 'data', + 'dlqr_reference_trajectory.csv', +) +EXPECTED_STEPS = 91 +STATE_SIZE = 6 +ABS_TOLERANCE = 1e-4 +SHUTDOWN_AFTER_SEC = 30.0 +RECEIVE_TIMEOUT_SEC = 25.0 +DISCOVERY_TIMEOUT_SEC = 10.0 + + +def _load_reference_trajectory(): + with open(REFERENCE_FIXTURE_PATH, newline='', encoding='utf-8') as csv_file: + rows = [[float(value) for value in row] for row in csv.reader(csv_file)] + + if len(rows) != EXPECTED_STEPS: + raise AssertionError( + f'Reference fixture must contain {EXPECTED_STEPS} rows, found {len(rows)}' + ) + + for step, row in enumerate(rows): + if len(row) != STATE_SIZE: + raise AssertionError( + f'Reference fixture row {step} must contain {STATE_SIZE} values, ' + f'found {len(row)}' + ) + + return rows + + +class TrajectoryRecorder(Node): + def __init__(self): + super().__init__('external_bridge_trajectory_recorder') + self.trajectory = [] + self.subscription = self.create_subscription( + Float64MultiArray, + 'env_data', + self._callback, + QoSProfile(depth=100), + ) + + def _callback(self, msg): + self.trajectory.append(list(msg.data)) + + +@pytest.mark.rostest +def generate_test_description(): + package_share = get_package_share_directory('external_sim_bridge') + launch_path = os.path.join(package_share, 'launch', 'bridge_sim.launch.py') + + bridge_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(launch_path), + launch_arguments={ + 'backend_type': 'fake', + 'max_steps': str(EXPECTED_STEPS), + 'min_subscribers': '2', + }.items(), + ) + + return ( + launch.LaunchDescription([ + launch_testing.actions.ReadyToTest(), + bridge_launch, + TimerAction( + period=SHUTDOWN_AFTER_SEC, + actions=[EmitEvent(event=Shutdown(reason='bridge trajectory capture complete'))], + ), + ]), + {}, + ) + + +class TestBridgeReferenceTrajectory(unittest.TestCase): + @classmethod + def setUpClass(cls): + if not rclpy.ok(): + rclpy.init() + cls.recorder = TrajectoryRecorder() + + @classmethod + def tearDownClass(cls): + cls.recorder.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + def test_bridge_ros_trajectory_matches_reference(self): + reference_trajectory = _load_reference_trajectory() + + discovery_deadline = time.monotonic() + DISCOVERY_TIMEOUT_SEC + while ( + self.recorder.count_publishers('env_data') == 0 + and time.monotonic() < discovery_deadline + ): + rclpy.spin_once(self.recorder, timeout_sec=0.1) + + self.assertGreater( + self.recorder.count_publishers('env_data'), + 0, + 'env_data publisher was not discovered within timeout', + ) + + deadline = time.monotonic() + RECEIVE_TIMEOUT_SEC + while ( + len(self.recorder.trajectory) < EXPECTED_STEPS + and time.monotonic() < deadline + ): + rclpy.spin_once(self.recorder, timeout_sec=0.1) + + received = len(self.recorder.trajectory) + self.assertEqual( + received, + EXPECTED_STEPS, + f'Expected {EXPECTED_STEPS} env_data messages, received {received}', + ) + + for step, (actual_row, expected_row) in enumerate( + zip(self.recorder.trajectory, reference_trajectory) + ): + self.assertEqual( + len(actual_row), + STATE_SIZE, + f'Trajectory row {step} should contain {STATE_SIZE} state values', + ) + for index, (actual_value, expected_value) in enumerate( + zip(actual_row, expected_row) + ): + difference = abs(actual_value - expected_value) + self.assertLessEqual( + difference, + ABS_TOLERANCE, + f'Bridge trajectory mismatch at step ' + f'{step}, state[{index}]: actual={actual_value:.15f}, ' + f'expected={expected_value:.15f}, abs_diff={difference:.15f}, ' + f'tolerance={ABS_TOLERANCE}.', + ) + + +@launch_testing.post_shutdown_test() +class TestBridgeLaunchShutdown(unittest.TestCase): + def test_all_processes_exit_zero(self, proc_info): + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/src/external_sim_bridge/test/test_fake_backend.py b/src/external_sim_bridge/test/test_fake_backend.py new file mode 100644 index 0000000..bda00a9 --- /dev/null +++ b/src/external_sim_bridge/test/test_fake_backend.py @@ -0,0 +1,75 @@ +import numpy as np + +from external_sim_bridge.backends.base import BackendValidationError +from external_sim_bridge.backends.fake_backend import FakeBackend + + +def test_fake_backend_zero_thrust_matches_reference_first_step(): + backend = FakeBackend() + backend.initialize() + backend.apply_control([0.0, 0.0, 0.0]) + + state = backend.advance() + + expected = np.array( + [ + 21.474377061136369, + 2.292875120797660, + 21.081978135884185, + -0.001232341729441, + -0.050158638812040, + -0.002059582195653, + ], + dtype=float, + ) + np.testing.assert_allclose(np.array(state, dtype=float), expected, atol=1e-12) + + +def test_fake_backend_known_thrust_matches_linear_update(): + backend = FakeBackend() + backend.initialize() + control = np.array([1e-7, -2e-7, 5e-8], dtype=float) + backend.apply_control(control.tolist()) + + state = np.array(backend.advance(), dtype=float) + + x0 = np.array( + [20.0, 20.0, 20.0, 0.00930458, -0.0467472, 0.00798343], + dtype=float, + ) + ad = np.array( + [ + [1.25645279151274, 0.0, 0.0, 349.682205848334, 147.7805051, 0.0], + [-0.0716204647063059, 1.0, 0.0, -147.7805051, 318.728823393334, 0.0], + [0.0, 0.0, 0.914515736162421, 0.0, 0.0, 349.682205848334], + [0.0014040831838806, 0.0, 0.0, 0.914515736162421, 0.809100657054006, 0.0], + [-0.00059338484671504, 0.0, 0.0, -0.809100657054006, 0.658062944649682, 0.0], + [0.0, 0.0, -0.000468027727960198, 0.0, 0.0, 0.914515736162421], + ], + dtype=float, + ) + bd = np.array( + [ + [63868.7072544296, 17836.8364281426, 0.0], + [-17836.8364281426, 61074.8290177183, 0.0], + [0.0, 0.0, 63868.7072544296], + [349.682205848334, 147.7805051, 0.0], + [-147.7805051, 318.728823393334, 0.0], + [0.0, 0.0, 349.682205848334], + ], + dtype=float, + ) + + expected = ad @ x0 + bd @ control + np.testing.assert_allclose(state, expected, atol=1e-12) + + +def test_fake_backend_rejects_wrong_control_dimension(): + backend = FakeBackend() + + try: + backend.validate_control([1.0, 2.0]) + except BackendValidationError as error: + assert '3-element control vector' in str(error) + else: + raise AssertionError('Expected BackendValidationError for wrong control length')