From a22e932edea824c4bcdcb82c1695519c692fd7bd Mon Sep 17 00:00:00 2001 From: Dylan Date: Thu, 9 Apr 2026 13:49:53 -0400 Subject: [PATCH 1/7] docs: define generic external simulator bridge contract --- src/external_sim_bridge/doc/design_note.md | 206 +++++++++++++++++++++ 1 file changed, 206 insertions(+) create mode 100644 src/external_sim_bridge/doc/design_note.md 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..5b25ef4 --- /dev/null +++ b/src/external_sim_bridge/doc/design_note.md @@ -0,0 +1,206 @@ +# 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 existing DLQR reference fixture in + `src/DistributedSatelliteSim/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. + +## 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 From 61b552b12411595fcc063126823eb26b17ec2c63 Mon Sep 17 00:00:00 2001 From: Dylan Date: Thu, 9 Apr 2026 14:13:23 -0400 Subject: [PATCH 2/7] feat: scaffold external_sim_bridge package --- .../external_sim_bridge/__init__.py | 1 + .../__pycache__/__init__.cpython-314.pyc | Bin 0 -> 203 bytes .../__pycache__/bridge_node.cpython-314.pyc | Bin 0 -> 8071 bytes .../__pycache__/config.cpython-314.pyc | Bin 0 -> 2191 bytes .../__pycache__/main.cpython-314.pyc | Bin 0 -> 748 bytes .../external_sim_bridge/backends/__init__.py | 13 ++ .../__pycache__/__init__.cpython-314.pyc | Bin 0 -> 939 bytes .../backends/__pycache__/base.cpython-314.pyc | Bin 0 -> 2336 bytes .../__pycache__/fake_backend.cpython-314.pyc | Bin 0 -> 3770 bytes .../external_sim_bridge/backends/base.py | 23 +++ .../backends/fake_backend.py | 57 +++++++ .../external_sim_bridge/bridge_node.py | 147 ++++++++++++++++++ .../external_sim_bridge/config.py | 30 ++++ .../external_sim_bridge/main.py | 11 ++ src/external_sim_bridge/package.xml | 27 ++++ .../resource/external_sim_bridge | 1 + src/external_sim_bridge/setup.cfg | 5 + src/external_sim_bridge/setup.py | 28 ++++ 18 files changed, 343 insertions(+) create mode 100644 src/external_sim_bridge/external_sim_bridge/__init__.py create mode 100644 src/external_sim_bridge/external_sim_bridge/__pycache__/__init__.cpython-314.pyc create mode 100644 src/external_sim_bridge/external_sim_bridge/__pycache__/bridge_node.cpython-314.pyc create mode 100644 src/external_sim_bridge/external_sim_bridge/__pycache__/config.cpython-314.pyc create mode 100644 src/external_sim_bridge/external_sim_bridge/__pycache__/main.cpython-314.pyc create mode 100644 src/external_sim_bridge/external_sim_bridge/backends/__init__.py create mode 100644 src/external_sim_bridge/external_sim_bridge/backends/__pycache__/__init__.cpython-314.pyc create mode 100644 src/external_sim_bridge/external_sim_bridge/backends/__pycache__/base.cpython-314.pyc create mode 100644 src/external_sim_bridge/external_sim_bridge/backends/__pycache__/fake_backend.cpython-314.pyc create mode 100644 src/external_sim_bridge/external_sim_bridge/backends/base.py create mode 100644 src/external_sim_bridge/external_sim_bridge/backends/fake_backend.py create mode 100644 src/external_sim_bridge/external_sim_bridge/bridge_node.py create mode 100644 src/external_sim_bridge/external_sim_bridge/config.py create mode 100644 src/external_sim_bridge/external_sim_bridge/main.py create mode 100644 src/external_sim_bridge/package.xml create mode 100644 src/external_sim_bridge/resource/external_sim_bridge create mode 100644 src/external_sim_bridge/setup.cfg create mode 100644 src/external_sim_bridge/setup.py 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/__pycache__/__init__.cpython-314.pyc b/src/external_sim_bridge/external_sim_bridge/__pycache__/__init__.cpython-314.pyc new file mode 100644 index 0000000000000000000000000000000000000000..704474da90f884c774e027526ad9c86488da479a GIT binary patch literal 203 zcmdPq^MASDe9K@24fL5#`_noLznt`#MzMR|!i3dNbZ zr8$Wu`9%szMVTq-sR{*&$=QkNsd|2zjJMe1<5TjJ&ryk0@&Ee@O9{FKt1RJ$TJpgACS6>|WI56p~=jJFvyidcXg0FKZ% A`~Uy| literal 0 HcmV?d00001 diff --git a/src/external_sim_bridge/external_sim_bridge/__pycache__/bridge_node.cpython-314.pyc b/src/external_sim_bridge/external_sim_bridge/__pycache__/bridge_node.cpython-314.pyc new file mode 100644 index 0000000000000000000000000000000000000000..b7a658df8958fe1d744735bd6678466335c2974b GIT binary patch literal 8071 zcmb7JYit|WmA=Cnl0$MR>Pn7tXVeRDc z%c-~)JEth&yULycAy=Uc7&Wj^OSamTtLIl0Y4w|Xs$VOn$g^_SAyPD zIb%8}+=&PSzm5O+*x(-~#q0!tSc9LxWgGn=AyH@xkgqdKObDhk%=D6n>}SZUI}}+< zDGA)vg!~YX!m+TTEQd7}MfT{YR3$Pjn>!_`v1M~Nsx5OOoDG zb@oG*JfO@(ZJ;xMMmp^7Cs;h_qty;H6S`I;@NvOEtjo2d!%D`OH8M||vAAZNBj19n z5F*FTo#`U&WZt|S1op*hpCbwL5V3UzW$t;P)v>1uUMt3zqz=KsU1erTq9o-Ofj4?t zQ9_7?H;*N2mdvzTbh(~yv*-|a%WYj97*}Jp!?-SsF4yy-MThLb(PO=p>k3)*Rx5BG zwCHjTtyNY$bEwQ=QNTH1(dGI$Y8E^&>t=&Odd6qb<@&fu&J35^EGp#KXC6l^j&d!K zkaHjIqh&6O0&adEZfimoE7ZX4I_k8}y9SSV6iwzG>Eq=2`B&DHKKO!c|y!A zBaoxEvh*d8pJdK5F(yV%vmGSH1U-5~l?#~m80ca!pXS{M|U!*?WAlauOr zx*pzQ2}w=eRwGL6wydb*Iui`Cf3eo3plZ@0*sj~vR8m%SUXo&om?lXI?wRg14XN&v zcH3%TG=E!nUAPmGlZXbodr{V;_{!oUNV3b+a{DBhxvXj+Xn8rDi0XDaKd?7wpXh$b zQN7lbo=n}2$J8Yl>cpRSBLe^gG?&0am6g?4L{{)7>#fof$c)QU#nzd>!{a7OsNkzMLKwMMsjw#*P*=ZdwLRtwRz{DE+PtEIDFo<8Qk!{|);mq4)-T@5cb(35oz8TQJvvvk+q`H0m*rgUFPW9t#*^!A6@}DD~3j^%$Y`quy+28fsf!>A29Pq&@ zF*_m<`=N1&4ROQh4A~VtAl+g3843c0f|goA08tvD2r^0&R0cE~=Z;fRTA)CysJ26~ zMxGJY?tg;CX1s`1W@MVaDhN0c(>eo5XJ=sO47>n>nrI1F$jnsoQRXu0C2e!InG)`q zolsfkvy05ZiXJjof~>+ryK({w-HmD(12-%!L;EPUk72vhyb6X!9}ta|^-C5v$dA=>nx1rMdn%8TLJ-~HZYelT1r_@I!RQH+i zltOkmDHcx3VVsmxK_^w!(v6iK>QKmN)p83sXMze9aFFIG;ITfK7&(&{l zd4k2Kb^S#<)I|qrI8<~Z#S?ee6Bkg%0%1X=$6TOm=K(Bv4ICkGX{^PUnD#sJTj0lr zY$K+;BnxA?ia{nhgSK_gGsl);?VCr+P7wgRErD7=9{^3SMVD(!k!Dce_dFrzJZe3C zV06iuvD(UNI#}Vxl;<*Q0C;Hz^CbuEm<_=EBn5Ns1@o%n%z%5x?FRD@w`--gl&u3| zt0Z@p>ow~NnCCykJ=UFY{WWDJ{*cujtKm54m+q7AB;|-EN2R!&Sk#tu&nmpGC{iN4 zET`M26TlaXiY2@fO}crF!0K$rXmqpRTWfW1&>v?gSQfw z!Q4>`a30$yu(*f?f}Dz$<-mahqG<_+;8h>q#}e!Z*7V*yhLLNe=ytmTh32+=^Wkjs z;ex;Oo$vniy9Kek5a=&DiGOg1aQ>S0ivTg+x_g&5E*l{A?nq8JRuqVM5KTvW*E*l$ zTME3e3xo>7*mh0B{fT_zp={%!t(xHHEaM$`%Casu+{oINb>A0H{iN>Tzq&KNV^0YS z^Y{wJoO|5=*q`Z}&Izv;y870qazdcc(EgLFe=RzJ|4X$47V(3xy~2_Y*t+vB_JhWO z^BnuZ83yRza&{3!wRdVsnC4Wm5`F^g$~CKkkAZ z#f=ABVRlqjc*OWDj^j-Oo+?1|JaB;#75CbZMaWF)K3gq>jDl@2Lf;U98^=JfSL)nH zO34+lhCNzgpK*UEE|dbc@{d#tQ$cDeP%U$oZ7lSo+U|LvwVM1Y;jFc$TMmTX z68d0>RZUy^_xupN&Rc3mW|qBopO#_wWWi^y$2-6iCAhNG)E zLO}&J0nv;{X*6oN!EnAN^nvw+pw0xoqbTFh^MS7%U{X{4ed^*Bd<}V@2!9!$2=u-0 zZG129JCXIB*z%1$>if;$uLtv|=CY^ea=y7g)HWByzV(Tm(DA9qcW-QCEbr;gdb&5y zmxm^v&N&)s@Ryg=Bmp?R?Jv5dR#tV%t`Nm+jF<9`ozw`D_ z-_H97v;M&?{}4s#UO?%l0l<`|n)M6CMl%r1^IciKD<{7EUMeS!=lC;U#pbiP_Vh+N zQ`7U{Lbj#{!MWE0X9y?V|Kz?O{)z7%#DGmS24syX?v4Q`A8Q|`-%me|XF{`?>u+Rc zZ)E!BbHdHN`|ycSPa(VEOBHS7Z(GmRO?HyscX}?4v%e3Xb6p%|KOD8g%Pzb|*rIri z4$|{tM2J2hQ5@oj!Fde2Xau56!rW=tc)`)1C-?EQTgdJS29a;w$bkZWCEB3D)YB27%+%2p=7F!0>fsvV*JO|ES~B@{LcRllqAbqi42}L$t}n@#LxXB2yqnDN)~{G-vj_ppS7vIPb0FZ( z@u$$_R?KY9UM*u-mfA&LyM~GGPBLeQQ|HpHqT!K?%ixZSOW{>nG-3iV?HMfj_M*<* zNVnfKm~Mj&#lRcfe9QM2gS^h6F6b92fbO33&SRyLl(e2QZl65bYAB$jc0Z|R^H~9Eq>)|^dMv1Uj@wewdXV~so&NPYADg`Wr39owFo_0=DpelVQ#1h?zKjqbevQl@M4 zQCDVk`lI&T_181sl(w#aD>EC;)ZX4S}-sf-iXeFJjD%V7`Kpu4PO2-FqIJplE08X z#W)YXptvPzc_o^H)TD&pR^Cj7<7SUfk``i$s>NdoIk5tn)^3#7i^UKYc#Rdhy-kof z#P4hoDXeLb!c9RU6~uYXv?738T#~>nd<{m>Jk#G9XM>)Bf(1qmD#mwT`~!JrgYPp% z{7Ccp8AvmI?uPWk=SLwxcs2%BUX-JhkEVT^#G^q`rcKfVA&Wzx&?I+J7`CrF=a*$%k)r4K;VD=!%rYgA<_ z41FWz4D`7UqImbY(ha+`Dgy$SaQzCd z9pd`CAbL^Z@TDUP7Qs*nNMeH_h{wTdU`~AqQr$->4C35t_OAQKcQrGNybRjZWHh$vYg2M8H$))TYB-gRfk z6>xGtq+WZZRSz6`NdJVY|AbeLw9$x4t=g*PmNb>pW8bXpd?2Et&SCa_&HM6uKTl>e zX#{8XkI4N|Md(+_bWfr~Y%c=QK+DL&H_!~`cvRBL4P{2<>L|L8$B?C3+9lk!HxclO z9I}!Zl(ytT#}U^o9ptGTs`T@O)$;}_re8>15sRLZ&26V*2LUkJoL{21fk>`0{|WV+ zj)VBqaf-Ij0c%5I_Yuby;>vQ8tAoggG`Y5j-olmwfl?4i<;g|FbxVVQ3DNgHb4z9V zf$y!lv54-t@t)CD$pjMRc$UZgv2c@xeg~Jkio_q;_ZMr zXXztOnP*o>;8w{>+<{lhoVYDhwa2255Zm*7ZgWbA7~0>psr{-XL7!uG^K4<_ICHE(>_Y^{D3)!ZPq&4D3s2weC6a%!CF&?yf^@&Y_)kP6H5w zD(eA_&^*`xkz|zDnO9VpoX9vW$aVneINR}dwlY5^udGxY zhpT~hP(L=dwiK6WV-X+E$A4?j|EHB-R9B5Y?S4D z|7oKxqdl7?88(cQBgdmeFOH$`WWs(9)5v-b!>MK9KMFyYjv7A@7p=-@%yaOjaqhRr?`j zy+AUV15BS3?+Koq*#)OUwI1-o3N4fw1xR28{=oJMJ}XeUodxa}&OJH*5`MkLn_`a` zC{b8RMoGRmXo^+8ZQ@$q?M-Q;%6T4EUq7T5(^~xPC1E@&L0GP2dE<>ajrQtMV zK5N&gumaR5l5M1mLGolF-XviFo|Zc3NZNWf3C8haHGycL4J{RE$3yM-(`#!N*R}I| z{FOCjU7OtHo5N?;>~-zjZ<(PDb0{)Th32WXY|Fe9nX{oe+cIxP=H1Y|yI~AQ#zbgL zJiFI2-i(at(3ox+*CXRjXx!Pzk3{*?Vg7V;@@gypPLy8=^9!x~ttfAYdHZi&%O)Ei zZuX&6HqvvUo_jjBt`9%Y=9@#;TiKarYGy;vMEZ$PKk*E&>tnIzW-B|_OwIk=JJ7fp z!@*oJDSCJRerdquXo@s*Dz(Q`5{LHCC+-HzfxZoX5-mmQY8$Jsu_<8X#s-quR58Xs mp|Ri5F#HoJyv!4= zVGl@u0Z9St5V94}v|20dk)^FBgl=fpA$01HUC5ZWLpBeAcSr}@0V~JV78lVo$&-z= z!!Srju+XS{e->QQz_ysQoK93@Z4B8=3;5zK!o6+nE^f`DPWCa29K%TGnm5xLs%XR--H=hbB8In2Bi6K4eI3#=HPE8seJW6 z@;ThKuK;I(waZE;mK&-aIHfB)87kp$!L_7WN?&dEbdtXivVvP9?{zs!UmVg+P9Zfi zEg~sk-x+bvR}fBJOWCAQ~J|58-t!`L18RDW>M z4jA4qC#i7roQGTb^TR|UPg2vExmb>1z8oiKTJrC8^Q4!h_vIywk+$kGx+9x0_6;U} M!t#kXd`5HZHwtEsn*aa+ literal 0 HcmV?d00001 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..46be163 --- /dev/null +++ b/src/external_sim_bridge/external_sim_bridge/backends/__init__.py @@ -0,0 +1,13 @@ +from external_sim_bridge.backends.base import ExternalSimBackend +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() + + raise ValueError( + f"Unsupported backend_type '{backend_type}'. Available backends: ['fake']" + ) diff --git a/src/external_sim_bridge/external_sim_bridge/backends/__pycache__/__init__.cpython-314.pyc b/src/external_sim_bridge/external_sim_bridge/backends/__pycache__/__init__.cpython-314.pyc new file mode 100644 index 0000000000000000000000000000000000000000..4d33440a726ce8b03591020d6dae7700c8fe48e3 GIT binary patch literal 939 zcmaJjCLcOe(P<2`j7 zkDS^z_v>iwd*W>KSgm1CASTj~Rpj0@9v!H_x>EiKP)FMstg$U-u_}5l$Wp4wd{9!b zt9nYh%xzpWvcsJgm%89#Tll6T)gTmts^bLuW>YkrFrdb_<$NG&B_2;|2X4dO5N`c7 zztR?04@$P}`fh03#nz6@*|y{R&Cm(CZHpXMO(6%Pcy!<#GEDzX$LAYobRC-aBejQa zhyWmgh_WI8KA=DVtR~QqH>TP6UZu!?DDkyeg;`L3j^OnXcvY$#Q4%o$)pTOBSOs#p z&>3|$?zhEERcf2Kr`?ffe$Z~Unj+-&{AFzOh28N&F~9J}aXn|luRIl)5hN9Vfya3Ya^fDcQa!@vqQ(( zJA2i=mF~=1H)HiwrB6QtC8djgEVN>#k3SSQ(NEw9gLs^&jIk4#KY@jF9e+%e@dum1;3)tA literal 0 HcmV?d00001 diff --git a/src/external_sim_bridge/external_sim_bridge/backends/__pycache__/base.cpython-314.pyc b/src/external_sim_bridge/external_sim_bridge/backends/__pycache__/base.cpython-314.pyc new file mode 100644 index 0000000000000000000000000000000000000000..c6d741cd82982220ea9a32a3c2031ed213e22605 GIT binary patch literal 2336 zcmb7G!EW0|5Z$FHN|x=|Na7?-lX_z#4%9gEp+(VyQy@iz+EpR643I+udqa^cn>ZN+72mg4|NbX@H(Ovm|Y~R?u`0!_jg$JM-qvtZtv1t`S)6 zm%oL#ON9J^KZ;e(6gq5!vQIXNOV`O7)pQC&35?RhdsZV3Fx|Ru)OU`U*|zG@bO~IMJRClUUWQwEC?~bmAvbT74j%^hN9oA1#YT zGldSXg0fFGh^8*l)~2INF4eYc>9Sh_|8{!mOV9s7#KA)^3IpT6DP=3B49sm zi$_4592MSAP~V4$^0bNGKtvk@yD#|g^_9;l6KumtDMJPPjH^rIPWDvKSF(q z^Cx{T%HPaz-VLSFQ5cIjf!7tzgTx1Aj&nRH^tq?C3_E=-6z8%E84WSLbX7@zMLg9) z#$Lo#*y9};23un6UY>$l>3B*scJ^ciTo~biqOge1$P4=`IaBPP0@Q+CSakR`$Pqwm z1)xwqC~Pzfz}lcgIYD<^2lR^3D{d9^38Potnx1ecp#W3q`DUKMgRqy^VsIbye&p#y z-cdWk54)jX)F8v$^EK;Zc{+PcZwt01VjWt`gW1Y9HNz(S{+$eyPadR>sSXb08Isfo3n zKbuTr)LIc)Ie0A*w7d-R zGUy{UkXP{C2OzK_>LU;tyiEgdlm0sM8eneH=LY5`%_7bz4b~Y=&cggzJ`@1tlU5f# z(2|V<7tmHL)CInlM6SsN5DL#)%0ZsZr-k5o#_8f1sQ+z*smXa1*r2Dl{lDOrH_%7M z=XV2n0gOg~Y#AV1^cRjmw&?SV$k*0;1G#!{XD7-!6}nm|T}({q;9`dQNw4R{(9s!f zyi zV15+AqH)01Osun1T)k)T$lE!)j#vWM9`t{}vRTC*KYF^%_LkXTneBqM3+nD*<8guD zhZqVTl@qa&VJy~8x;Tm literal 0 HcmV?d00001 diff --git a/src/external_sim_bridge/external_sim_bridge/backends/__pycache__/fake_backend.cpython-314.pyc b/src/external_sim_bridge/external_sim_bridge/backends/__pycache__/fake_backend.cpython-314.pyc new file mode 100644 index 0000000000000000000000000000000000000000..7acd6e92794e37033c5b03b75ef37a1f80c1bb6c GIT binary patch literal 3770 zcmbssZEO_Bb@ul5Zhhaejcu?gXFeP*wS56hF-eLEA%K5iuG`cR5s!~<8M5=0(Uya%y(3p=*-TY{RELx?pbw;S_pT0MH zdv`WQsO?Dm=FOWoZ{EDGnf-bw=p)cxT)dU-lL`482TpU_h)_BLNRbQ?Md&BT1WVY0 zxU1iFOteIfi~Vj(0-C$ubIfabw~!5_ohVW>Q9L~^TZ$zs;x-cV-o;@|V1CUo@|I?0 z^M(mS`P=%Lf^MYr0T?tL(o&~&BR#0)vT4rpJf(S>vL)HDELJED!njCYCYGQO%QfV) zL`AUNiVHRun@KKWNs1eAPzG?1;sM;tajzl+UdM4+@d57Vc%2fkd`b|M4Ke=_?X+Ij zX8PD+-O}k;*2tPxHWkff4PB$rVO!UzHL6+Bu`DWT!a$Fv3Y6-G71fQ2E+e1Tqv^Q|EZWjDlBZ*uWunNf?665waeWe0HME>+X2;ZFnoVc)b!lbo&G-l$R4tFX$ER38 zRm&dIRh6~W*rB_2CJ0-a4P5j4q*d}QBbeFFyAqHIKPw{G>fnQO)s;?xszs9ILG1Pe zo8@;mPyFCF&wXhxu<0!z1Z0j}`sikd@M7%@CQ0Uk>r(jfFYelO;M@Q4sb?>rd2X&M z)M1&+e0Hi&Vxc<@ZwAG^F`zU4`v1s57NobP?B z_ivE|i@d-h{VZ~lMUpHsuoUzLLn{%|(6Z9tYsB^PN)u^pvbp!O$WtuR$s#>0vKNpC zxZjXzJ*e`(6-0{VgyMN(0u4cD7eHr9b+8g8g==M;Y|ZLpnCigKRy zi@zc&?l`3NtVHp$X5MlbOoyFI^CccHFH=8E(*S};S)~h@OpM7Qxli0uXc@r=;U(;@ zfN`L+wXH3PXcKOn7x==Ya|$vj^px=-vG*0Yydb38NU~C*6=6Nz0NL$w*1$C%6ZYFeujEu)tq0f6a&yIDJtU1kbncbcpPAV;?{8bE-+80MFJ039Wdh)$3R+0}uRy#* z_ny>#bE&+!|Y9d6C?f1GnVO*X7MK znR&Tmp=HxMz6-vK`+f~ot@oV!jo^dc!83wY9{m`DijKL+Gj7_SgBPKbgOBpoK0KLI zk6hAj^v~{@R^C+=0w&%-8QV=?0D`Z>giT)sfm(4+b)vIkc!KL{Mh*{9D=u?rgw(F< ztdc>7Q3;XtcT8YGUel6jCf=M>PoRa?w4U;K8=<&m%uBaJN81tXK!8EPJerA586GCg zW98B84^(BZmY&eyM}jr13K5&gjei7WL2xu}IzRf;(Tk0jTHkHG(bV~Y^~=fG$t&CE zx<2f>5kGpP=_p=?x5Eu@o_+1?t#JGGaQpi^J_+x<6^>sI$3F@0x+d@9{))M6Wh;7e zwFhE;youq(kMC!ns*dH;g&gAWvOZJLa^;z@s*Yr-Y2n|5k%wuos;2WPRi!)e+*o+* zOnDlyX92JcDt{MbQ>tcJG&@|dbQAc|tXv`c0`%e#0Xo1$`@Kp&bu0JW@Ib%!6x@OL zJW%BC1)w_Imz_AY*W;>K9V$IifL{$znqnM+$*0D%MuuW2G07;5jZfJL+`i85Mz`~8 z0f?#dYRuJHqbHM}u`P3e9tW`)VJ1G4mPA1i{z|sqA+4X2`a9(CeNSB2T_OOulK%l< Ci3TMA literal 0 HcmV?d00001 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/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..f17a729 --- /dev/null +++ b/src/external_sim_bridge/external_sim_bridge/main.py @@ -0,0 +1,11 @@ +import rclpy + +from external_sim_bridge.bridge_node import spin_bridge + + +def main(args=None) -> None: + rclpy.init(args=args) + try: + spin_bridge() + finally: + rclpy.shutdown() diff --git a/src/external_sim_bridge/package.xml b/src/external_sim_bridge/package.xml new file mode 100644 index 0000000..89c6438 --- /dev/null +++ b/src/external_sim_bridge/package.xml @@ -0,0 +1,27 @@ + + + + external_sim_bridge + 0.1.0 + + Generic ROS 2 bridge package for external simulator validation. + + Dylan Long + Apache-2.0 + + ament_python + + distributed_satellite_sim + rclpy + std_msgs + python3-numpy + + ament_lint_auto + ament_lint_common + + + ament_python + + 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..3fd1bbe --- /dev/null +++ b/src/external_sim_bridge/setup.py @@ -0,0 +1,28 @@ +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']), + ], + 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', + ], + }, +) From 8330968ca35a4a6e9219306f19d6f963d102ceab Mon Sep 17 00:00:00 2001 From: Dylan Date: Fri, 10 Apr 2026 19:46:13 -0400 Subject: [PATCH 3/7] feat: add Basilisk backend to external_sim_bridge --- src/external_sim_bridge/doc/design_note.md | 66 ++++++ .../__pycache__/bridge_node.cpython-314.pyc | Bin 8071 -> 8510 bytes .../external_sim_bridge/backends/__init__.py | 5 +- .../__pycache__/__init__.cpython-314.pyc | Bin 939 -> 1103 bytes .../basilisk_backend.cpython-314.pyc | Bin 0 -> 12496 bytes .../backends/basilisk_backend.py | 201 ++++++++++++++++++ .../bridge_sim.launch.cpython-312.pyc | Bin 0 -> 1413 bytes .../launch/bridge_sim.launch.py | 67 ++++++ src/external_sim_bridge/package.xml | 2 + src/external_sim_bridge/setup.py | 1 + 10 files changed, 341 insertions(+), 1 deletion(-) create mode 100644 src/external_sim_bridge/external_sim_bridge/backends/__pycache__/basilisk_backend.cpython-314.pyc create mode 100644 src/external_sim_bridge/external_sim_bridge/backends/basilisk_backend.py create mode 100644 src/external_sim_bridge/launch/__pycache__/bridge_sim.launch.cpython-312.pyc create mode 100644 src/external_sim_bridge/launch/bridge_sim.launch.py diff --git a/src/external_sim_bridge/doc/design_note.md b/src/external_sim_bridge/doc/design_note.md index 5b25ef4..1ec2391 100644 --- a/src/external_sim_bridge/doc/design_note.md +++ b/src/external_sim_bridge/doc/design_note.md @@ -167,6 +167,20 @@ For v1: 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`. @@ -204,3 +218,55 @@ documentation: - 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 implemented against Basilisk's published Python API +documentation, because Basilisk is not installed in the active development environment used for +this slice. + +Documentation 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` + +This is sufficient to implement the backend boundary and fail-fast import behavior, but it is not +yet a substitute for validating against a single installed Basilisk release in CI or in the +development container. + +## 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 + +## Current Runtime Requirements and Blockers + +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` + +Known current blockers and gaps: + +- the active workspace used for implementation does not have Basilisk installed +- the backend therefore has startup validation only, not a live end-to-end Basilisk smoke test +- the exact Basilisk release to support still needs to be pinned and tested as a single runtime + source of truth diff --git a/src/external_sim_bridge/external_sim_bridge/__pycache__/bridge_node.cpython-314.pyc b/src/external_sim_bridge/external_sim_bridge/__pycache__/bridge_node.cpython-314.pyc index b7a658df8958fe1d744735bd6678466335c2974b..437a784e5d5efd50ead6fc11e64d50df428c7ed7 100644 GIT binary patch delta 1550 zcmZWpZA_b06u!4_-}dcyq2HfSKFUgq(DJd3%`ujiF>#}*tckIt@=@enYALtv%t#Ex zACB=u=I)=xXkucP_(!vanCTA}jBYVelWnrVD#rbE`a=jH%lzZHEgfdw~>J-1G1{aTF@!MORys^~Y|&_0y--Mn%W0bFe;i7n8u##vs~RV={rRP=qBsH^BP9vBUkzkS&O`?3K$8p;~eqdqz) z_2Fhf6AsWtsS8`^XVNAdqz|Ni`T}pJX5LB@Aj7;BcTvonmI&X8v6Fr(o6{#rA(I#1 zOeH5q#B_nAmb`K+#wTfyFeI#ny5^-Tf{Rk&7WUDY;-KLe`tF!2669_x>LUT}c7Trr z>4%CL*v+E+$kqktDk{Z{m={OV;!MiNk;7mjFVdfs5#Sh9zu>N=zf?a;j&?FCpD$$7 zDIZ^Eg&8%%pr1h#K#m9SdcC!^#LR510L50iVRF++c@?bHr0c`>LB?b;nM1p%mbY#) z_&5xLUevqrxut9RjKtIegy%-0l^!tspjtr4iL#K&sDWmUc4{@|C5cAbU<_IR8|=h^ z%JgV6&AN>A9b+7yq7RKJ!AhFJM_Oo~$y0ZTsRtSO7<2#>Wn&|mu~eFPnWd9nGP%Ul z%q|}pKhHd*kvZ6vi|SZ8BN0NT2=Ox$PsY;O3q>I}S;&u1y_G4-vxU*Ln42VWa95%n zuPl*cmGUfR?etGmi>wr*&#Bw|0oKy5%zfB-#d22eLVDXalGB9c=-Ks#wzcZe^^Tj} zH@dF}z8qaPEz9rf`?okrqjo;LqV{adkfwU^%)*%!weweX^*RF&QLz#t?F>%S!_L5oZcuaVi61YQn3lO1=$V5p_T*i(D}ko4!8*gv zam?|S9i@f0?u4MscEgxnb7l@*V(dx{DzSSL0bCv`31S217<|u$6f{T@-nvf*TsB<{ zWw8S#Bzt~3t2SvoD2_VzT2Bpqzse%Z;y8*bphBHhah!tbP87$JIELz>&|jA_0+eux zFe{=XoC}GV^uU_%z)H6o2E8cFWyMUntkmPFT%Jr_NM@$SQ$9&iHcG_tH&R92b3Gx) z4z~9Miz+rhS!R-1aFlM&Za0V4HFZC08s_@eX3h4w!3~L&cdrYYMdgC>a@UGre`K#+ zwFmFngY&vILAxqA?g|71gs9G?GC9|*3eITdX5p1D^* z-w5trFXuBCmC58pArGx9nIsStM9v{ZPr2LiFn!zo37(~%8W*0YJvBkgd5mpW!`q0- LUBW?zN~!r5XzE`k delta 1101 zcmYLI&uWdQRQv%HS#Uvc;nx9rKt&~8;KTuOs7e$n~exP*GQzfaT7rTcdOu2tS%bc!@E`nuTrBb$NzO z8Q3a&z>j}Rdb6u86Z{t9-QhYjtq3lHKdByFtS6gcxwcwF@l+JQl4JVzI`hekTo0|o z-((duyeUUvH@=j6p%2~4PIM_6PAfhfRKRltd5Sbu5XUM$})7xcH`}nmMy*&SlM|OibcKg5txtt|o+L zuk#tiD|*u-TWEl!O0ksBW@1WBb&#Z^1W5$94|bv7Js|W)-DBWCBKU$wK$&f@4#l@h zaCuZXyxI~3R{6apXM?V4DWf>$c?h%E(Y}l!?<5?;uf3VZcHTpK@4yxB*2o}*dkA6# zp9`=W(sQ|VCd<2t8Nh%ROiU7QpPN5T8Qw*SXqC#BPGrr(DQ*|D`YmVc=>qkqI~?X3 zG5z?fwzHvn{&V<5`x#VR)W@J5Z|JjjFKaR0G6~y!!`ojpwf*_L# ExternalSimBackend: 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']" + f"Unsupported backend_type '{backend_type}'. Available backends: ['fake', 'basilisk']" ) diff --git a/src/external_sim_bridge/external_sim_bridge/backends/__pycache__/__init__.cpython-314.pyc b/src/external_sim_bridge/external_sim_bridge/backends/__pycache__/__init__.cpython-314.pyc index 4d33440a726ce8b03591020d6dae7700c8fe48e3..714e8dd77f34057e0199b52e1ed3620ec02b08ca 100644 GIT binary patch delta 479 zcmX|7O-lk%6us}wr=yLLQ9(H=N-~uMVjooW1ww6F3?va+O*)Y|WxP>>Rzbf&X4BF| zo9KVEO{lz2}_=;YpOI#ydJ)l_HbPuL38y)SlI`S z*7lMr#F45AyLSfXfZ&5+ZA`LC<0u=oKV~`VQn-}R`jzP}XyMl(Z5TJTeOd9=iCrD)- z_E;|)j(F6QVMlQoP1&nhnV*g%{@6rbSoQz5KQ~9R2|IJ`O-vP|=M5e9Vs^CQJRVM7GvJAqwE0S(K@F(+s6KgLrGZ{~!yYwEs n0mM@!E}vD3HPcujuFI|!R)|7}K?t>BtqogkSnu-GZm;PFo%(Q< delta 365 zcmX@lv6`Jvn~#@^0SF?WU(ejiIFV0+QEQ^QtXdGWI71MNI75y=2}=;GGJ_`DOOPr} z##=(J6(y-fd5Jl}nYj}?U#al`g;{~PSQn3k!Oje6w2Py=EAjU8T9)=($BW4puh)4#bCi5+pw8ZSx$vI52jPjGanB>%AHCb-4 z7MB!d7TjXZ$uCbWDq;buzQq-mm{XeST2z!@q{&poKKVYAiaa++f-ku!HL)Z$J}EId zJ2fw*NCYSdcA?8;C1zcfCtM;oxKtLzEbzNv6nKFn=rc1DH`@mWCRVm0UZ6@pP1akA zlOHh4vLz)Jrxr0!zRj%f1hNop&PoQbq9U+G95%W6DWy57c103Ed5{;11%Si{W=2NF Ry9^F@863Z}0Yd<+0{~w*S2+Lx diff --git a/src/external_sim_bridge/external_sim_bridge/backends/__pycache__/basilisk_backend.cpython-314.pyc b/src/external_sim_bridge/external_sim_bridge/backends/__pycache__/basilisk_backend.cpython-314.pyc new file mode 100644 index 0000000000000000000000000000000000000000..d0b45c08bd3ccdc90776fd650fe8012733219757 GIT binary patch literal 12496 zcmb_iTW}lKd0yOz1qhN5N%105!tyG@yo!=6*_K67G-c8-8v0?6({ zA{lqWNpE!9X`CjFnNHi#4;{syK$Nei6%*Sm2j=hTj*12n(=osZj z9GW9UuyfQo;?i6rZq3cgu2IiOiB=Ng4s%VM=&t2N&nc(TgXR@WTDY+HS5yo;^wLNo zk<=nuJeg3#PTe(_Sk?I{`73ET5tUJ2b1@QqPfo;UB1`cYLmO6>=+r!-ODhrG__#;o#S+aYdNqF!C$>&Fv{K}4$O9q|sC~k5 z(kc1?9cK_>buftV!VC%kdX7P5fI0~}S)!GR<(OziEw>cZ%EchyLkzAED*;zAI4D*F zKFr`lVh!M023Lx8fa@7tB_09Xz~E}J5%5t49~O@R4l%e!6aY6dxK?b|>ckc-YL{Md zF`~wo;_7?(Rf{ZQnLDDgovu9ms4!FUi5bhF+Msx2lH z!rb%oR=aiaaA&Bqt5#MQ~8BHxVDJOy z_^dIX{Z-eEs{BOcl~hupO%Nxh1eF$q<}v%uN|&(OC9HJ`*8yDzczx~NSxk=2Ud03e z%Ze7qMCTP0^WE7SBt$eVu3^=wzjz`}sPQ!-sid$bD@iuAL=r^>r`bT#lw>R&m1A9k zn#B6dC1b1Itap6X*k2*Cg2`f>s{*=Xv~?q{EwWKr17bKt#o8jiZBibxbAVyKnIrahoJvzy?wgZLv!Lizd<^8G5rDtT~Q)nx=W!jkWbJ{Ka?a`N4o* zJ(p)gk}(=di>UAEwFlp(lw?#^RsG1p3Y*CE`h#^Qp{Tz!xc=_InzM6d&Do8A=PGji zJJ*n}MIFGPKRqOVTqy1}0$Ar}xNFX{zsGSgyldpHI2IkELv;2!TDVVLzam_{M3J?$ zlF;4b*p^R8RFz-{u$wA%*{qMKO0-vAF?fLl9+aPJ(RX>ytM=NnF?exLYE=(NQeFtk zlB73Y+|R;$3TylEX|;;Y;B(IB>3vAaW-H6~@S0>>??WqmGbhL`h=8nj#<%Q$b+vbuoHcB}HzJlpDaL+Y0)IcW5G7&WT_3C5HJHKZ0D!)vq7KM7lCQfu8lc)$c9Y_n3X1M=VqTN=Ztfb zsOZ4+g#Kx74m`jlYB=!VZTW#$D4MFa`SlR(Rm0~S$H+TO*v`$B?Xq|#b}3J9DHqyU)Ni+$?6hGMGEdiJxB14-XIy5FSaXYl(6?aY zn9yXO3EBDNNgFmHwC*usJ2$`c#1%hn5;@(?HTkO4m3XY=fmJhQGUVioBALI(c0 zrmxqU{QH1m09&$}gmT=l3@kRU*P9Kbm|;p-!hngP@oia@V;RgU(OSocO^v1wA3$}CJ)^h?u&Q7yA?V`x;MY3dN98as$}Ye zJedy?bxIt)T@VCA1p93(LqBp*2-|f`tbK&zwa(=#YqFKSnabXs$`c#j+>xfO&bvn- zT_J*Nk8Q5pt?gg}xVmAx@xDMx+ zU)u>?W`g`dL+E=ieD{TH!|6=J>07bv^COw(M|K)Uw;{&)x-2hbcp(=Eeq;25(QKe4 z6KL6b<&%kzC#ajz%<0jcz-ynmoId|9&ov6!hW<=L|E)8(&)q#qeHh9>b9=V=Y^M2a zuBl@?{PNwV^SSo!?VeZfwvX(3xRw|8I9H3GsSCS)&R6yg-v>V9t6SBdggy@4YR&eK zX8K3(@UP|gK(X43nf{A+_@M_?wb`oPOjYl#s#~6ablrX>d+ti++?AcP;@v)Rr)p}0 z&$Wd=kv^8PZKIjC(Ve!-*|sa0wkzA>+dFOV&;Xk8fh-@&@S!Xp&hX(Ze>%gnHJQvj zKY540^1vKGdxmen$9LGOMV%${xLZ}tS%&AfRghLNBBHpeYsJdNDt=eWX=K|u*{qHD&5)LpzSYCQd3v;Reh zk2}UOUmpJ}n&H}jc_J-86I>5)(!l192q(LlHv>jve%FNS%zx7*C~{PeUkB2f&+1^U zyqnpA3*9!4^HgEaW1(J7ntjSYS&Uy#Q03`-fKmOdJnME)J!11mT! z8!QSu%3x8!yy+}kR*YpO=Us9=2Qe58egmDabN5TjH*Wm#xy=)IN*nK2cH}Cr+!5c{ zo_TjiytaKs+78a{xtvwyyPUJE{4<_wYTcT+_2xa{Jx!Z=X6U2T*fKZKSw z@^jW0Bx@Z*yvRTHfrPH&2y$;Y)6O0)?Ysp2Hs!Pr=8Sa!$DNnKT!^&OPH61~_AtIc z-#qY4T1U{X0A_Kkqs+!JAyY3hkCmB2PvTlIh7cP^ocq)<9`-74qnR>82`Q>f_$n6( zI*&xJHfc`Ml2S6k3}!X#V&axjukseqR64Bf?Rk1Fi=B4%-CVfCxBukii#v5MegDkOw!b{Qnf~j>@1M!ky#yIr#?YEGd~=rX$nYIE ztFz(ZOnCT5vFs%&b4l8{6xj*S-Q%PG{U0u_{+vp@?>p6lUGCq^>V-RjBm?9BYo&05 zYo!C}N@?8eWm3X-&4(dcy`O+ZTz@-LO<*jA6l@%Ayi_i4mO$I}Izq!y*zY&Jpx{ zLxjQzVgu7_&DmJY$f#h4^`~e7HdI=XEe&N#Lq9G(e!ueAc4%g&^3CnQn@~BkeDja_ z=B-$^{l!fCi+6eaNK-Xb;g$>!jXc|WKGS;s4nMH({T;sTL9jX-6f!{}8|=vhd$Pfk znc&Ht;OPy|zXc9`*!EG^H@h~!^5?xff#&=F&*j}RZ_vNXd3}DH?U;g%771+U=`L*W ze47uJ$Kp}#9aU4h1jJU~eX2x$A78OpVI{FbBsY6lW{(L->>(DwSmB=nSmzq7$Z)D4 zGMsXbxKl1WGMsXccvGGcK7|OeSTgMvz0)3%pLU48=@QXD?G#I=UA-V_fxBus)k?S+ z;6^Uxy=pnqM7McFr@f0#(UbBHc;k*Z_mb-<7k7ZHX~p0Gg{q6=p^(!==_S~ubP-G* z7KI!fYhN}=%{n-6|1#aROPEV*OzX5z^3hC$C*O-Ku)r3DM=1OmS)vFN;!+fEimMde zfqsjCiGtU8dw0Cx+cYpXfP%4=>W9rQ^+Xhejx>xTt;lQ^dX-fN=SDt+YB$y^t?IOf zIItFn&tm^n^(y!R;L8|KYL}AfM2sDFy>yg0JXj&j8yW)}oV8{LyYa{rhF8hqMg}@P zhIhFiZ}bjx9Olt|l1GJ6TO3W+xm_pktN&@>%+CT7KMTB_3B0`<$5U0*zy_Y_SN67Ji6<296AAg7Rqu7S6#pB zb(Pis&#nuVzgCH)zFybC-7Fn+yZ#|C;QFBlz&?$Gge5y^0vlQJU=i$5C)uO059qY% zhip%kWbCy<{TSBh(-e>ofoU0{<({U89Fki@yOiA?Vx`A*;QTOsd+&oqKo4;g(}68X z3vITTpK+!@qYydxsf%a=wwE0-BnM(h#jz~Y)rd49bxk;sx{ zR7s=bqtl~`vp0+k8EoPuaE?GHovaCRbBi2LQwJKvC~FQ&4RCd7wdOzg@4p+h0%<*hd`|B@Ur_AIRS4U>U<*Q1I1?Q+meI z2gn#N#%Dcr7MS3!F?fgb3wehNyrr;pz8j0Tkjuzz^=$!gTVYbnZ8bCFE*8lXDJl&v zF;g<*DR5^MbzYgmo+9;U@_Zx2T|Q~p>*$|O*n#H>T!msTwE730Q~5c*W`h zB8m5;>HOuCOK>Q8mLhZVlI|OetQfEL3f#{bizg=2OR7xTF=GP=anzd44umH33d30? zE0=IXC8?|`7cdUJ#5fj)eNR@lUP0l2WC3R=vprPc3ryvPSAd;IMn*(I`XRG-E3lyl ztylB;FMddvZgOU+Y=cumfNvGEAO5FmQ`b{Jd-G1)q^V!C+Oyk&2;{=XN zuAw*EFpy~&*l8GqOB~+vY(w9jhQ1FjZw%)GjfP7+7ZmhhC>N~G)im5c654WSg$o(s z!p@PwoY1yg#+9}HC+96If9T>GPwZB6KAd}UBEYSl4IIk^j^zSJ8Qci}*JoZgyy<=} z*i`g%KC|k~E*3sjNB#j#oen9TKR@ZnNVeW%navtd&mPNNaLzbtMUDI$AU&n>Nx{J4 z$E(W{Rd8pyZND(Wu~rax#$6b6EW`=J#bo-IpNQpr7My45$@L69xt`n;GjSbEl0xBP z?k@^6D(s-RmLn^W!E_LG?~253O6lHJ6Ijaw9yN-KODd@;1>`PKtJcTK-MEEP=u2bU zPA3Oqsjv5&_xeA9+^^EhZY8*Ix2+itqE9V3~Jk-NbWvK1>n^lnz|1deWb?grZK zqgeYPzuB@AIJR~8ZlIlB{5JKr@@vzaDUOI4gQA`-qJpIh(jxGt(@+^l(GW${(NoXE zzW~Y7UfrJBha8ey2%{kT5wuri#t`WonooX+zP&%gn4nUJ>w?Lg zQw8L1nTpU2E$z`93I~4Qj>2WccggolI-PP2FR(Hc^3@xP#X$3yN_C%{z(rSF{ge$? zN11W0RHRGHs+Olq-~z`^S#ojTPHfo3(^{h@vNOCe(r`X$qg|DHNy(C-wy zwW`APbRmo4Fu0KAm+}S|U|gpAN;Oit2dGS0r${ZmT^a{^)DlxH#r3`*B4G0uaDcUVwMhmEZ%9Dsh4E zQ4Q{WJ$fF&&PPWm>hj1B@Asot_ 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] + self._thruster_command_msg.write(self._build_thruster_payload(self._control_to_on_times())) + + 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() + for index, on_time in enumerate(on_times): + payload.OnTimeRequest[index] = float(on_time) + return payload diff --git a/src/external_sim_bridge/launch/__pycache__/bridge_sim.launch.cpython-312.pyc b/src/external_sim_bridge/launch/__pycache__/bridge_sim.launch.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..590f1ee35f6d61017b6f9e7f4dff6f06d1d0d6fc GIT binary patch literal 1413 zcmZuxOK;>v5N?k>wlj}pvYtnVY&O=SNNb2PSwKR&T37)AA+gDY+i;3LI68E)bTN+h3ymOrU4BTFc<~$SrqPjZpv`E zU)hTYb;{K8sSZ+xcRB;{5+zcOf*=y95cym=6IK`}^R_Bci8g1G=KOTBKx%WgN>WCE zn03;Cx0zInn1bc3WSqmQZ@}~$j4b{jyVCj6SsCL*o{Qfi%kVVM@0=+G!-lAW%wFd#vX5su=9e3#7>UJl*T*q@r3{%+Hpe1F|Ry&c)Y!r1|!PMXx|(a47p9jr-#%@ z6KX!#Ha{{w=w9e0)Z$Bl7us<;63qwCm`e|jKimalkOpRW8;ynsHo*xt=eC+7x*suW zVsd~(#|vF<(8GkX5c_uQ1$L1EuNSi&!VTdn@K73BGSi`UtRciA2&>AqG1LmXl-G(> z@yMrK32^{kQJRd?gxAKH!5TPX-X~t1K+bR`V#he4z7Gcy#9VhnC#SO%W(bD15FH`9 zf=gkdiA)tjG@FTg1R1pEyJchM^5$)M^QG~>VhNBfiwJ!Xv4~}g@%qW&c@)sWxA7om z&fpDI8Tk1}Hz;cZS#K4{*7%68x-_KlX;Hht?75D!dqC0?Y`=gwMVGzJOz&o0GiyJc zY08TFN@>p;sPj&y8(H_Qto;R`9rbhUYH`540erEn z0CvPrCC;0~+a)OOUE(EaVG@<*cgpIpDBim9$$KhF^b=z1FtIfduk|NEL{guA%r;;t h{tNL6h-+Drq`&VYY2ylQU!f1K(A!gWQ_^zJe*rTfh^qhq literal 0 HcmV?d00001 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 index 89c6438..ad7c768 100644 --- a/src/external_sim_bridge/package.xml +++ b/src/external_sim_bridge/package.xml @@ -16,6 +16,8 @@ distributed_satellite_sim rclpy std_msgs + launch + launch_ros python3-numpy ament_lint_auto diff --git a/src/external_sim_bridge/setup.py b/src/external_sim_bridge/setup.py index 3fd1bbe..7df2cb1 100644 --- a/src/external_sim_bridge/setup.py +++ b/src/external_sim_bridge/setup.py @@ -12,6 +12,7 @@ ('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']), ], install_requires=['setuptools'], zip_safe=True, From 6913f008e848952ad76e087c0ae578c32aa4f695 Mon Sep 17 00:00:00 2001 From: Dylan Date: Tue, 14 Apr 2026 11:36:21 -0400 Subject: [PATCH 4/7] test: validate external simulator bridge compatibility --- .../external_sim_bridge/main.py | 5 +- src/external_sim_bridge/package.xml | 4 + src/external_sim_bridge/pytest.ini | 2 + src/external_sim_bridge/setup.py | 12 ++ src/external_sim_bridge/test/__init__.py | 1 + .../test/data/dlqr_reference_trajectory.csv | 91 ++++++++++ .../test/test_bridge_node.py | 158 +++++++++++++++++ .../test/test_bridge_reference_launch.py | 165 ++++++++++++++++++ .../test/test_fake_backend.py | 75 ++++++++ 9 files changed, 512 insertions(+), 1 deletion(-) create mode 100644 src/external_sim_bridge/pytest.ini create mode 100644 src/external_sim_bridge/test/__init__.py create mode 100644 src/external_sim_bridge/test/data/dlqr_reference_trajectory.csv create mode 100644 src/external_sim_bridge/test/test_bridge_node.py create mode 100644 src/external_sim_bridge/test/test_bridge_reference_launch.py create mode 100644 src/external_sim_bridge/test/test_fake_backend.py diff --git a/src/external_sim_bridge/external_sim_bridge/main.py b/src/external_sim_bridge/external_sim_bridge/main.py index f17a729..a30db5d 100644 --- a/src/external_sim_bridge/external_sim_bridge/main.py +++ b/src/external_sim_bridge/external_sim_bridge/main.py @@ -7,5 +7,8 @@ def main(args=None) -> None: rclpy.init(args=args) try: spin_bridge() + except KeyboardInterrupt: + pass finally: - rclpy.shutdown() + if rclpy.ok(): + rclpy.shutdown() diff --git a/src/external_sim_bridge/package.xml b/src/external_sim_bridge/package.xml index ad7c768..8263b46 100644 --- a/src/external_sim_bridge/package.xml +++ b/src/external_sim_bridge/package.xml @@ -22,6 +22,10 @@ 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/setup.py b/src/external_sim_bridge/setup.py index 7df2cb1..cbff7e1 100644 --- a/src/external_sim_bridge/setup.py +++ b/src/external_sim_bridge/setup.py @@ -13,6 +13,18 @@ (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', + ], + ), + ( + f'share/{package_name}/test/data', + ['test/data/dlqr_reference_trajectory.csv'], + ), ], install_requires=['setuptools'], zip_safe=True, 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/data/dlqr_reference_trajectory.csv b/src/external_sim_bridge/test/data/dlqr_reference_trajectory.csv new file mode 100644 index 0000000..e33bbf4 --- /dev/null +++ b/src/external_sim_bridge/test/data/dlqr_reference_trajectory.csv @@ -0,0 +1,91 @@ +21.474377061136369,2.292875120797660,21.081978135884185,-0.001232341729441,-0.050158638812040,-0.002059582195653 +16.235280829810684,-12.626303436833686,17.897073550814067,-0.026227831720655,-0.029245513266003,-0.015377821951063 +6.486265440934633,-17.928792370983569,11.604173281188201,-0.026246256566643,-0.000210193261359,-0.019075908867384 +-0.921495986127911,-14.657820888936183,5.126211415129908,-0.014339968125821,0.016724520136396,-0.016391037186507 +-3.849430758681117,-7.964165980281448,0.167265629767840,-0.002345851112268,0.018792486112254,-0.010759271242311 +-3.380891646081291,-2.214866308767135,-2.687034547248812,0.004150239405856,0.012243603636776,-0.004868068419918 +-1.591165474919844,0.791424514717339,-3.632177709694075,0.005122266877151,0.004322674674491,-0.000306605690358 +-0.043614681468529,1.374181850224460,-3.257640824468955,0.003134742848019,-0.000808406882247,0.002357201198349 +0.664617082282504,0.729638777597417,-2.228566701235212,0.000743898443748,-0.002439505804749,0.003276996280556 +0.657906206414624,-0.073743443877466,-1.086264874954650,-0.000696980811848,-0.001823112190483,0.002977124704970 +0.330971721040675,-0.508581945651428,-0.166113575865198,-0.001020720543594,-0.000547582318208,0.002060718548973 +0.015329011059273,-0.530645138707978,0.397375313967736,-0.000680298454477,0.000377611359071,0.001024392242183 +-0.143237538571929,-0.329236505905397,0.617328280461929,-0.000197655023774,0.000674126056910,0.000179853809020 +-0.152863852920315,-0.105741122595624,0.587311293554424,0.000120831878710,0.000523171378039,-0.000344197298124 +-0.088233598208714,0.031500529500849,0.423053791634292,0.000212536717138,0.000226513992327,-0.000555115183179 +-0.019092823861825,0.070106967906883,0.223842397382815,0.000156163687870,-0.000004184846688,-0.000535570424320 +0.020151822277853,0.048841001418285,0.054748201126866,0.000057963862955,-0.000100286848925,-0.000390223036746 +0.027637660377120,0.012405480178353,-0.054875140357848,-0.000013439959627,-0.000092191013124,-0.000209966531991 +0.017362253957798,-0.012388246937803,-0.103289119715632,-0.000039136555874,-0.000041974125598,-0.000055100788509 +0.004024606637649,-0.019278778224219,-0.104852444897458,-0.000031964748966,0.000002694850871,0.000046541557838 +-0.004266674868684,-0.014181831153614,-0.079453435073491,-0.000013271501684,0.000023018822655,0.000092518431997 +-0.006230665384917,-0.005674057243120,-0.045130586172460,0.000001765353938,0.000022152975767,0.000095399721206 +-0.004303784126565,0.000578215803262,-0.014351205462328,0.000007952564771,0.000011720416460,0.000073117886306 +-0.001463175685113,0.002967409253729,0.006702437549975,0.000007115515519,0.000001669424568,0.000042151149912 +0.000480705384147,0.002534796430046,0.016962350857774,0.000003425527393,-0.000003559064543,0.000014022041491 +0.001112578343366,0.001061489667573,0.018528740408490,0.000000152452438,-0.000004170256100,-0.000005446033351 +0.000855998414737,-0.000162854126861,0.014772089186698,-0.000001395657131,-0.000002416107405,-0.000015121692885 +0.000320885002231,-0.000676263216993,0.008938341191861,-0.000001430148236,-0.000000431363051,-0.000016818171799 +-0.000084269762665,-0.000610022086421,0.003389079963565,-0.000000760233584,0.000000706095743,-0.000013564123234 +-0.000235393091205,-0.000302536431028,-0.000607687484002,-0.000000087671257,0.000000908515668,-0.000008318243224 +-0.000196770752819,-0.000025539625252,-0.002722033057431,0.000000265986983,0.000000581114914,-0.000003257833008 +-0.000088227219720,0.000109222545469,-0.003238751759931,0.000000304924340,0.000000162142484,0.000000428789750 +0.000001842616754,0.000116751578098,-0.002719870958324,0.000000180314705,-0.000000102964871,0.000002412091037 +0.000041380523015,0.000063424998307,-0.001743749380732,0.000000039055105,-0.000000173626043,0.000002932190398 +0.000039395852415,0.000008031988677,-0.000753018135335,-0.000000043459497,-0.000000122624223,0.000002492079204 +0.000019523911640,-0.000021750412277,-0.000002369227561,-0.000000060305606,-0.000000040488042,0.000001617735714 +0.000000974234777,-0.000025489667143,0.000423940411196,-0.000000039256040,0.000000016783630,0.000000716316457 +-0.000008122836629,-0.000015385146913,0.000559435231812,-0.000000010888854,0.000000035402972,0.000000025519878 +-0.000008538715507,-0.000003624410311,0.000496032772930,0.000000007362721,0.000000027393249,-0.000000372649367 +-0.000004748239115,0.000003391380662,0.000335724111448,0.000000012314101,0.000000010893977,-0.000000505043149 +-0.000000801535449,0.000004950656525,0.000160645034296,0.000000008806165,-0.000000001742916,-0.000000453517632 +0.000001364282241,0.000003293209478,0.000021080047797,0.000000003064056,-0.000000006665637,-0.000000310602928 +0.000001698521967,0.000000957630118,-0.000063356065400,-0.000000000998119,-0.000000005744219,-0.000000151686158 +0.000001040568890,-0.000000577239587,-0.000095359510000,-0.000000002377098,-0.000000002590837,-0.000000023533219 +0.000000242067852,-0.000001015155690,-0.000089603810618,-0.000000001878888,0.000000000088603,0.000000055045766 +-0.000000241501604,-0.000000740604133,-0.000063876663961,-0.000000000758473,0.000000001280685,0.000000085810778 +-0.000000351003668,-0.000000267579503,-0.000033272683170,0.000000000115983,0.000000001225477,0.000000081746512 +-0.000000236583113,0.000000073921247,-0.000007575060745,0.000000000463499,0.000000000623364,0.000000058948387 +-0.000000073150011,0.000000194428025,0.000008897841463,0.000000000404719,0.000000000054302,0.000000031241020 +0.000000035806373,0.000000157127238,0.000016009509005,0.000000000187164,-0.000000000231238,0.000000007695475 +0.000000068497989,0.000000065392334,0.000016029302559,-0.000000000000572,-0.000000000252262,-0.000000007587105 +0.000000050912271,-0.000000007781194,0.000012022117856,-0.000000000086073,-0.000000000142353,-0.000000014352296 +0.000000018673079,-0.000000037776737,0.000006735679242,-0.000000000084417,-0.000000000024519,-0.000000014591041 +-0.000000004998987,-0.000000033755994,0.000002049002385,-0.000000000043666,0.000000000041183,-0.000000011068591 +-0.000000013529978,-0.000000016014526,-0.000001122621297,-0.000000000004297,0.000000000051900,-0.000000006296100 +-0.000000011073868,-0.000000000349568,-0.000002639913351,0.000000000015811,0.000000000032328,-0.000000002011098 +-0.000000004733099,0.000000006973609,-0.000002838611662,0.000000000017576,0.000000000008110,0.000000000923222 +0.000000000397924,0.000000007021436,-0.000002239683610,0.000000000010093,-0.000000000006803,0.000000002355919 +0.000000002554019,0.000000003717763,-0.000001338543713,0.000000000001904,-0.000000000010411,0.000000002577836 +0.000000002330667,0.000000000442556,-0.000000491918470,-0.000000000002743,-0.000000000007138,0.000000002057451 +0.000000001124219,-0.000000001270033,0.000000111548094,-0.000000000003572,-0.000000000002261,0.000000001246539 +0.000000000038473,-0.000000001456203,0.000000425828069,-0.000000000002262,0.000000000001044,0.000000000474149 +-0.000000000478228,-0.000000000854325,0.000000497314260,-0.000000000000591,0.000000000002067,-0.000000000082761 +-0.000000000489611,-0.000000000175402,0.000000413183772,0.000000000000455,0.000000000001559,-0.000000000377855 +-0.000000000263834,0.000000000218211,0.000000261889161,0.000000000000719,0.000000000000590,-0.000000000450486 +-0.000000000035937,0.000000000295222,0.000000110432020,0.000000000000501,-0.000000000000132,-0.000000000378744 +0.000000000085322,0.000000000190856,-0.000000003148275,0.000000000000164,-0.000000000000401,-0.000000000243110 +0.000000000100470,0.000000000052757,-0.000000066773849,-0.000000000000067,-0.000000000000334,-0.000000000105241 +0.000000000059873,-0.000000000035503,-0.000000086118198,-0.000000000000142,-0.000000000000146,-0.000000000000669 +0.000000000012941,-0.000000000059196,-0.000000075499862,-0.000000000000109,0.000000000000010,0.000000000058805 +-0.000000000014701,-0.000000000042095,-0.000000050550707,-0.000000000000042,0.000000000000076,0.000000000077792 +-0.000000000020433,-0.000000000014377,-0.000000023728653,0.000000000000008,0.000000000000071,0.000000000069059 +-0.000000000013411,0.000000000005112,-0.000000002568483,0.000000000000027,0.000000000000035,0.000000000046793 +-0.000000000003861,0.000000000011622,0.000000010075986,0.000000000000023,0.000000000000002,0.000000000022435 +0.000000000002337,0.000000000009110,0.000000014721082,0.000000000000010,-0.000000000000014,0.000000000002997 +0.000000000004070,0.000000000003652,0.000000013665007,-0.000000000000000,-0.000000000000015,-0.000000000008779 +0.000000000002940,-0.000000000000575,0.000000009640373,-0.000000000000005,-0.000000000000008,-0.000000000013256 +0.000000000001032,-0.000000000002241,0.000000004941122,-0.000000000000005,-0.000000000000001,-0.000000000012472 +-0.000000000000331,-0.000000000001946,0.000000001037337,-0.000000000000002,0.000000000000003,-0.000000000008901 +-0.000000000000799,-0.000000000000891,-0.000000001436669,-0.000000000000000,0.000000000000003,-0.000000000004644 +-0.000000000000636,0.000000000000013,-0.000000002479592,0.000000000000001,0.000000000000002,-0.000000000001066 +-0.000000000000261,0.000000000000421,-0.000000002449452,0.000000000000001,0.000000000000000,0.000000000001231 +0.000000000000034,0.000000000000409,-0.000000001818302,0.000000000000001,-0.000000000000000,0.000000000002225 +0.000000000000153,0.000000000000210,-0.000000001004525,0.000000000000000,-0.000000000000001,0.000000000002231 +0.000000000000135,0.000000000000020,-0.000000000291181,-0.000000000000000,-0.000000000000000,0.000000000001675 +0.000000000000063,-0.000000000000077,0.000000000186374,-0.000000000000000,-0.000000000000000,0.000000000000940 +0.000000000000000,-0.000000000000085,0.000000000410488,-0.000000000000000,0.000000000000000,0.000000000000287 +-0.000000000000029,-0.000000000000048,0.000000000434684,-0.000000000000000,0.000000000000000,-0.000000000000155 +-0.000000000000028,-0.000000000000009,0.000000000339434,0.000000000000000,0.000000000000000,-0.000000000000367 +-0.000000000000015,0.000000000000013,0.000000000200322,0.000000000000000,0.000000000000000,-0.000000000000395 +-0.000000000000002,0.000000000000017,0.000000000071209,0.000000000000000,-0.000000000000000,-0.000000000000312 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..746c076 --- /dev/null +++ b/src/external_sim_bridge/test/test_bridge_reference_launch.py @@ -0,0 +1,165 @@ +#!/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('external_sim_bridge'), + '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): + rclpy.init() + cls.recorder = TrajectoryRecorder() + + @classmethod + def tearDownClass(cls): + cls.recorder.destroy_node() + 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') From 85c3237baa5a06acb1db42262b303ce72e2ae0bf Mon Sep 17 00:00:00 2001 From: Dylan Date: Mon, 20 Apr 2026 23:49:46 -0400 Subject: [PATCH 5/7] cleanup --- .gitignore | 4 +++- .../__pycache__/__init__.cpython-314.pyc | Bin 203 -> 0 bytes .../__pycache__/bridge_node.cpython-314.pyc | Bin 8510 -> 0 bytes .../__pycache__/config.cpython-314.pyc | Bin 2191 -> 0 bytes .../__pycache__/main.cpython-314.pyc | Bin 748 -> 0 bytes .../__pycache__/__init__.cpython-314.pyc | Bin 1103 -> 0 bytes .../backends/__pycache__/base.cpython-314.pyc | Bin 2336 -> 0 bytes .../basilisk_backend.cpython-314.pyc | Bin 12496 -> 0 bytes .../__pycache__/fake_backend.cpython-314.pyc | Bin 3770 -> 0 bytes .../bridge_sim.launch.cpython-312.pyc | Bin 1413 -> 0 bytes src/external_sim_bridge/package.xml | 2 -- 11 files changed, 3 insertions(+), 3 deletions(-) delete mode 100644 src/external_sim_bridge/external_sim_bridge/__pycache__/__init__.cpython-314.pyc delete mode 100644 src/external_sim_bridge/external_sim_bridge/__pycache__/bridge_node.cpython-314.pyc delete mode 100644 src/external_sim_bridge/external_sim_bridge/__pycache__/config.cpython-314.pyc delete mode 100644 src/external_sim_bridge/external_sim_bridge/__pycache__/main.cpython-314.pyc delete mode 100644 src/external_sim_bridge/external_sim_bridge/backends/__pycache__/__init__.cpython-314.pyc delete mode 100644 src/external_sim_bridge/external_sim_bridge/backends/__pycache__/base.cpython-314.pyc delete mode 100644 src/external_sim_bridge/external_sim_bridge/backends/__pycache__/basilisk_backend.cpython-314.pyc delete mode 100644 src/external_sim_bridge/external_sim_bridge/backends/__pycache__/fake_backend.cpython-314.pyc delete mode 100644 src/external_sim_bridge/launch/__pycache__/bridge_sim.launch.cpython-312.pyc diff --git a/.gitignore b/.gitignore index 22c572e..bc50341 100755 --- a/.gitignore +++ b/.gitignore @@ -2,4 +2,6 @@ log/ *.log build/ install/ -**/.DS_Store \ No newline at end of file +__pycache__/ +*.py[cod] +**/.DS_Store diff --git a/src/external_sim_bridge/external_sim_bridge/__pycache__/__init__.cpython-314.pyc b/src/external_sim_bridge/external_sim_bridge/__pycache__/__init__.cpython-314.pyc deleted file mode 100644 index 704474da90f884c774e027526ad9c86488da479a..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 203 zcmdPq^MASDe9K@24fL5#`_noLznt`#MzMR|!i3dNbZ zr8$Wu`9%szMVTq-sR{*&$=QkNsd|2zjJMe1<5TjJ&ryk0@&Ee@O9{FKt1RJ$TJpgACS6>|WI56p~=jJFvyidcXg0FKZ% A`~Uy| diff --git a/src/external_sim_bridge/external_sim_bridge/__pycache__/bridge_node.cpython-314.pyc b/src/external_sim_bridge/external_sim_bridge/__pycache__/bridge_node.cpython-314.pyc deleted file mode 100644 index 437a784e5d5efd50ead6fc11e64d50df428c7ed7..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 8510 zcmbtZYit|WmA=F0kQ|D7l6p|$=w+L-EctC!vFtjQ6{+z;-w}%$;-3eV^}~bNAP|odnYR|6PdhZzAN+_+lihMlf4zKrEB%B*aXSNk(A; zNSmfiiV2?PDf6U7vCuNhly%ak*!1sq#SY)CQ|zQeanLf`lylOhxF~I(;wIgSo6_u5 z-K0nHOx7#)^xZMlFxjXyPBtk`0n$RcNyzCVA=fp>QM1-3r8&ewiQ7jK^-4>~)JuYO zpW&w8$c%nC2+WI79wteB5wUa{W$s8&YTHo+uT}NSQiovSE-_a~vMl8~fj4?s zQ9{TJZ!SYt$OLbk@pgqwcNlc#IfKhcQ^<1NX&fY|=P}BmUbjJ4o-;iL9kK#PpYc|C zR>{H%rwKTN23>hVX^rtu4^%h|3OEN1y7D}V`wSBF+NV)SPnSnvl);?pW&ziRERFbG6#(l$-}u$CnCzC#A`=b6C=j>eYzeMlv^orEW-`* z3z3_GtcWRjEYk>Y@uVQ9Z^%(8enXVxF_j4h&HrhPO9}u*v!GwK%ITCSsjMKxlW|25 zB%CwVuB%nmBW%~)z^J#aI?mmUiYdef)j20BLSk`l4kTGcs>eM7^jwq`5VWumNybzw z?H_0#)K+vp#F$zyNa>^!Ul0YHnY1LT^?G}$^o>MZo(Exe?2r?y0DwSo8BAnRT8c+S z2~WD(Cd@-C2~nt;Ouc0mr&W-pC6&{KV~dhnPagsdKfVax#i-h72+_VM@#yzdo1k%9 z;>p=Xm8+h4)lr^y)w5&HgJ#tti-}ojFZRwM2+>4DmIXmZy|#y6mb~zkP)Vp=#)2c{ zl>r1)u(l#p{k{YW^XW3#WZkRw9NU_08_Ki8C5zS3xY=>wz1T*_k(Kcuo-g|Og8yL7 zfAD_u`~F|~H~god^uqr*2|v7I$+P}qi+9EHLub+7jjwIR!^iR2`LMmK&_0}NAI={< zvp)KNqijz@7|nuFMPax ze)W8T3+A{W?`H>2Tlnta>)swgTIgW4lTw#@$)$?Ed-8PiG39$8(*> zv;GtJ-&`O6===xg3*)cl#$U^xy;>s73==2@dVl`rPv0yAUdRPr$abB4aHeE6xljME z+2U|MeHnw2CdUns)1M$JPvVG_B+(eOU_!4S48o!WH-~Hs^ z_$l&lr(EX_nSbZ#JvV6n-JlidZK!PlK3FYgiw9yi)DD?LmSHrStP(DiYSZiy2?0fF z#0-H(YKAPxNGB~H zb8I_qUXORBPMZT9r!iJULApwPR6=dr3Rfv)g_SbHO4+oPvP)>p%1WlW+L9d#X{9PF ziE;|;UNO_Mt!j7*)iWkPwq|%Dn1Yg4ab!NJbXKOvh9b_~J3Xj9a{pn4|o}#VeVi zaygT3`OEcU3~W zSapW6x6}ud_CSG#TXs1ejf%1?;TD#TA)^)Sn1l%Gj%t8$EI)_YBxVS9GHRARd-m|U zCK!fSb$Ay|up4O8bF&ULmr2R#boh(j_JVhR&bz

w5S5KmLA^?=JfLOLpQL+#(iV z-O5VYKj*h?zn`c{#EVm7WirmS~x~4nhh30*^=6xG= z!OzW%d*HFz>~O+~tZ!ZMJbmmV4SRp#%zBPICT8fzGZ=Gby?@=8?VHMT-zWz9RwnbD zzu46Aqf1|w?7;tp+zEsD!Pj0ilMl@eXC3Abn+MKX%paa&fc~w;3i&q7L9I=-*;jdr zQax~fC9GTmrn2kCqxyktZJa(TsuY-LLM;G|JGQSyHcAK?0o|av$RPqJjsd5z{Baj4WnEBag!szHKZBe_y|-nTpu+Lj z%NqGtkn{~*Qe8!YK~19xup=3|5I~homz}x_lkvT#t^|j9x<-$|-7z1o($6?Q6c@Y| zxdt_jR>O`2QrmYF!D~gw!gt)ZI}()El8@fRj%1AKx(#8sj6Udm)!Howc2tMg8Ppmy zU137(2_5e$Jp@UA>s&gZPK~)70{89`bIi62+CZGAR;}yIU={AAP08|}#Rk||Y%old~Hc5r^s*#&nJ0T3WY5&6h0!~*$@4IE!^ZNXPXA5fZt z7Bl>dTKtgL9Z2e0g9FJ!edOjdrcoZllIUV*4$-21fX$maY|c{H+%>-zY?TI}9SQfQ z>P7!ncc`hy3tlbwtx^wij9_*GGYr#6xc9*eh(MGyp&=>4riQ)lNGg@Mg}yb~2pXtf zKz@59wuCh#To4UW#W#|wUH6iyk8Bv_VMB!OYU(h2 z1SRZyJ`7jfUJ4xlLJiKMr>WrK;g|LBK;M3Q_3eV^Sk7~7!*k+6-){zgJy;kG=SIVM zPxz1Z-Xh<(GM?u;AG$oZPp+OUxVm$$?zJ;{SO0r`zZ|$X@WApL?$=yl&t3i_*I2UDIhv4_!u{SH zxA#3uVc*HzzLR-wtk~RIXb$F@gGFD*yKnvYt%7eb=NsJc4N;q+7i@->0k9cb>Q>H` z{JL9OU;{ZekmnEGPv`luJbUU{OW`z*J+qq0*7e*ym#gbRQ=!)|6~MW5zT>DtTScbsv^6dOnj4&HvkKzxf$#sP2q9?GsRZC znp)`7O~S0vO4%L*U+bBkn!+};6~gQ&qD58D!q!@0(2CRmdHFKN^1Ddb3af6(wDFoV z!7qR(!OusQL|$`e_>60??3VBHyft!YJ_qhSy4AE@X4AbKhvC>jtTCR9i8oa% zl%z$~P36a<=of)QhVu>X3HT?Fv|t6*!FzVmjfPm=o1(k(mmp)AJmyHF_s;A;`B!Y4 zuDX?_e|X{UaNZT%Yy=OW>&}5}VB|p{J2LfgNB)&pv)>jrUinV;N+er<15R;MU5S|7 zb+Ct9d@GJkfB)L-AN&Va&##=@^mniI|LpwAh0P}4yJJ5YTk9+|4S&)!yy@kOzMeY^ zYlqfeS$iqFe`5Wu+{CxC&98zqUn59sXoR!j_7*sQj`QEW_6KhNBRAkP!nK6gU=&SLj32N${heb!Lw_=*6Qjn}!Qo zI0_O>9+AEa8H8DGK{w|lTwb*^23k(r;c&MA$}N!qO^BBtLvRy8?<^!bH%@BQ4dHZ1 zIPEGYD=}d~o|A_l({4Zf;UpaE(5PwthI< zG|ZqKBy<2(_N1l=PDv~93b;ySw0}!xhG9M>J%1*yPf5pL$npOm91x$9fxnOweVH diff --git a/src/external_sim_bridge/external_sim_bridge/__pycache__/config.cpython-314.pyc b/src/external_sim_bridge/external_sim_bridge/__pycache__/config.cpython-314.pyc deleted file mode 100644 index fb132e64c373b414e861de6a131067f1044c54d4..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2191 zcmbVN&2JM&6rb5$dmSe>$->4C35t_OAQKcQrGNybRjZWHh$vYg2M8H$))TYB-gRfk z6>xGtq+WZZRSz6`NdJVY|AbeLw9$x4t=g*PmNb>pW8bXpd?2Et&SCa_&HM6uKTl>e zX#{8XkI4N|Md(+_bWfr~Y%c=QK+DL&H_!~`cvRBL4P{2<>L|L8$B?C3+9lk!HxclO z9I}!Zl(ytT#}U^o9ptGTs`T@O)$;}_re8>15sRLZ&26V*2LUkJoL{21fk>`0{|WV+ zj)VBqaf-Ij0c%5I_Yuby;>vQ8tAoggG`Y5j-olmwfl?4i<;g|FbxVVQ3DNgHb4z9V zf$y!lv54-t@t)CD$pjMRc$UZgv2c@xeg~Jkio_q;_ZMr zXXztOnP*o>;8w{>+<{lhoVYDhwa2255Zm*7ZgWbA7~0>psr{-XL7!uG^K4<_ICHE(>_Y^{D3)!ZPq&4D3s2weC6a%!CF&?yf^@&Y_)kP6H5w zD(eA_&^*`xkz|zDnO9VpoX9vW$aVneINR}dwlY5^udGxY zhpT~hP(L=dwiK6WV-X+E$A4?j|EHB-R9B5Y?S4D z|7oKxqdl7?88(cQBgdmeFOH$`WWs(9)5v-b!>MK9KMFyYjv7A@7p=-@%yaOjaqhRr?`j zy+AUV15BS3?+Koq*#)OUwI1-o3N4fw1xR28{=oJMJ}XeUodxa}&OJH*5`MkLn_`a` zC{b8RMoGRmXo^+8ZQ@$q?M-Q;%6T4EUq7T5(^~xPC1E@&L0GP2dE<>ajrQtMV zK5N&gumaR5l5M1mLGolF-XviFo|Zc3NZNWf3C8haHGycL4J{RE$3yM-(`#!N*R}I| z{FOCjU7OtHo5N?;>~-zjZ<(PDb0{)Th32WXY|Fe9nX{oe+cIxP=H1Y|yI~AQ#zbgL zJiFI2-i(at(3ox+*CXRjXx!Pzk3{*?Vg7V;@@gypPLy8=^9!x~ttfAYdHZi&%O)Ei zZuX&6HqvvUo_jjBt`9%Y=9@#;TiKarYGy;vMEZ$PKk*E&>tnIzW-B|_OwIk=JJ7fp z!@*oJDSCJRerdquXo@s*Dz(Q`5{LHCC+-HzfxZoX5-mmQY8$Jsu_<8X#s-quR58Xs mp|Ri5F#HoJyv!4= zVGl@u0Z9St5V94}v|20dk)^FBgl=fpA$01HUC5ZWLpBeAcSr}@0V~JV78lVo$&-z= z!!Srju+XS{e->QQz_ysQoK93@Z4B8=3;5zK!o6+nE^f`DPWCa29K%TGnm5xLs%XR--H=hbB8In2Bi6K4eI3#=HPE8seJW6 z@;ThKuK;I(waZE;mK&-aIHfB)87kp$!L_7WN?&dEbdtXivVvP9?{zs!UmVg+P9Zfi zEg~sk-x+bvR}fBJOWCAQ~J|58-t!`L18RDW>M z4jA4qC#i7roQGTb^TR|UPg2vExmb>1z8oiKTJrC8^Q4!h_vIywk+$kGx+9x0_6;U} M!t#kXd`5HZHwtEsn*aa+ diff --git a/src/external_sim_bridge/external_sim_bridge/backends/__pycache__/__init__.cpython-314.pyc b/src/external_sim_bridge/external_sim_bridge/backends/__pycache__/__init__.cpython-314.pyc deleted file mode 100644 index 714e8dd77f34057e0199b52e1ed3620ec02b08ca..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 1103 zcmah|%}*0i5TCbSt));SMiL;)Du%j%KnO7n8UrL4dLg)m#%N*Pwh!5`-8OHxDkLU7 zc=Mm=!HfS*2^eq#iHTmgVS3=|yk!@O5}jn`&DWbZ`oiap^&8C9g+TOt#&~sXO4+Efs^xq9 z;XA>c$I6p5YToqhs##)o>ZN+72mg4|NbX@H(Ovm|Y~R?u`0!_jg$JM-qvtZtv1t`S)6 zm%oL#ON9J^KZ;e(6gq5!vQIXNOV`O7)pQC&35?RhdsZV3Fx|Ru)OU`U*|zG@bO~IMJRClUUWQwEC?~bmAvbT74j%^hN9oA1#YT zGldSXg0fFGh^8*l)~2INF4eYc>9Sh_|8{!mOV9s7#KA)^3IpT6DP=3B49sm zi$_4592MSAP~V4$^0bNGKtvk@yD#|g^_9;l6KumtDMJPPjH^rIPWDvKSF(q z^Cx{T%HPaz-VLSFQ5cIjf!7tzgTx1Aj&nRH^tq?C3_E=-6z8%E84WSLbX7@zMLg9) z#$Lo#*y9};23un6UY>$l>3B*scJ^ciTo~biqOge1$P4=`IaBPP0@Q+CSakR`$Pqwm z1)xwqC~Pzfz}lcgIYD<^2lR^3D{d9^38Potnx1ecp#W3q`DUKMgRqy^VsIbye&p#y z-cdWk54)jX)F8v$^EK;Zc{+PcZwt01VjWt`gW1Y9HNz(S{+$eyPadR>sSXb08Isfo3n zKbuTr)LIc)Ie0A*w7d-R zGUy{UkXP{C2OzK_>LU;tyiEgdlm0sM8eneH=LY5`%_7bz4b~Y=&cggzJ`@1tlU5f# z(2|V<7tmHL)CInlM6SsN5DL#)%0ZsZr-k5o#_8f1sQ+z*smXa1*r2Dl{lDOrH_%7M z=XV2n0gOg~Y#AV1^cRjmw&?SV$k*0;1G#!{XD7-!6}nm|T}({q;9`dQNw4R{(9s!f zyi zV15+AqH)01Osun1T)k)T$lE!)j#vWM9`t{}vRTC*KYF^%_LkXTneBqM3+nD*<8guD zhZqVTl@qa&VJy~8x;Tm diff --git a/src/external_sim_bridge/external_sim_bridge/backends/__pycache__/basilisk_backend.cpython-314.pyc b/src/external_sim_bridge/external_sim_bridge/backends/__pycache__/basilisk_backend.cpython-314.pyc deleted file mode 100644 index d0b45c08bd3ccdc90776fd650fe8012733219757..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 12496 zcmb_iTW}lKd0yOz1qhN5N%105!tyG@yo!=6*_K67G-c8-8v0?6({ zA{lqWNpE!9X`CjFnNHi#4;{syK$Nei6%*Sm2j=hTj*12n(=osZj z9GW9UuyfQo;?i6rZq3cgu2IiOiB=Ng4s%VM=&t2N&nc(TgXR@WTDY+HS5yo;^wLNo zk<=nuJeg3#PTe(_Sk?I{`73ET5tUJ2b1@QqPfo;UB1`cYLmO6>=+r!-ODhrG__#;o#S+aYdNqF!C$>&Fv{K}4$O9q|sC~k5 z(kc1?9cK_>buftV!VC%kdX7P5fI0~}S)!GR<(OziEw>cZ%EchyLkzAED*;zAI4D*F zKFr`lVh!M023Lx8fa@7tB_09Xz~E}J5%5t49~O@R4l%e!6aY6dxK?b|>ckc-YL{Md zF`~wo;_7?(Rf{ZQnLDDgovu9ms4!FUi5bhF+Msx2lH z!rb%oR=aiaaA&Bqt5#MQ~8BHxVDJOy z_^dIX{Z-eEs{BOcl~hupO%Nxh1eF$q<}v%uN|&(OC9HJ`*8yDzczx~NSxk=2Ud03e z%Ze7qMCTP0^WE7SBt$eVu3^=wzjz`}sPQ!-sid$bD@iuAL=r^>r`bT#lw>R&m1A9k zn#B6dC1b1Itap6X*k2*Cg2`f>s{*=Xv~?q{EwWKr17bKt#o8jiZBibxbAVyKnIrahoJvzy?wgZLv!Lizd<^8G5rDtT~Q)nx=W!jkWbJ{Ka?a`N4o* zJ(p)gk}(=di>UAEwFlp(lw?#^RsG1p3Y*CE`h#^Qp{Tz!xc=_InzM6d&Do8A=PGji zJJ*n}MIFGPKRqOVTqy1}0$Ar}xNFX{zsGSgyldpHI2IkELv;2!TDVVLzam_{M3J?$ zlF;4b*p^R8RFz-{u$wA%*{qMKO0-vAF?fLl9+aPJ(RX>ytM=NnF?exLYE=(NQeFtk zlB73Y+|R;$3TylEX|;;Y;B(IB>3vAaW-H6~@S0>>??WqmGbhL`h=8nj#<%Q$b+vbuoHcB}HzJlpDaL+Y0)IcW5G7&WT_3C5HJHKZ0D!)vq7KM7lCQfu8lc)$c9Y_n3X1M=VqTN=Ztfb zsOZ4+g#Kx74m`jlYB=!VZTW#$D4MFa`SlR(Rm0~S$H+TO*v`$B?Xq|#b}3J9DHqyU)Ni+$?6hGMGEdiJxB14-XIy5FSaXYl(6?aY zn9yXO3EBDNNgFmHwC*usJ2$`c#1%hn5;@(?HTkO4m3XY=fmJhQGUVioBALI(c0 zrmxqU{QH1m09&$}gmT=l3@kRU*P9Kbm|;p-!hngP@oia@V;RgU(OSocO^v1wA3$}CJ)^h?u&Q7yA?V`x;MY3dN98as$}Ye zJedy?bxIt)T@VCA1p93(LqBp*2-|f`tbK&zwa(=#YqFKSnabXs$`c#j+>xfO&bvn- zT_J*Nk8Q5pt?gg}xVmAx@xDMx+ zU)u>?W`g`dL+E=ieD{TH!|6=J>07bv^COw(M|K)Uw;{&)x-2hbcp(=Eeq;25(QKe4 z6KL6b<&%kzC#ajz%<0jcz-ynmoId|9&ov6!hW<=L|E)8(&)q#qeHh9>b9=V=Y^M2a zuBl@?{PNwV^SSo!?VeZfwvX(3xRw|8I9H3GsSCS)&R6yg-v>V9t6SBdggy@4YR&eK zX8K3(@UP|gK(X43nf{A+_@M_?wb`oPOjYl#s#~6ablrX>d+ti++?AcP;@v)Rr)p}0 z&$Wd=kv^8PZKIjC(Ve!-*|sa0wkzA>+dFOV&;Xk8fh-@&@S!Xp&hX(Ze>%gnHJQvj zKY540^1vKGdxmen$9LGOMV%${xLZ}tS%&AfRghLNBBHpeYsJdNDt=eWX=K|u*{qHD&5)LpzSYCQd3v;Reh zk2}UOUmpJ}n&H}jc_J-86I>5)(!l192q(LlHv>jve%FNS%zx7*C~{PeUkB2f&+1^U zyqnpA3*9!4^HgEaW1(J7ntjSYS&Uy#Q03`-fKmOdJnME)J!11mT! z8!QSu%3x8!yy+}kR*YpO=Us9=2Qe58egmDabN5TjH*Wm#xy=)IN*nK2cH}Cr+!5c{ zo_TjiytaKs+78a{xtvwyyPUJE{4<_wYTcT+_2xa{Jx!Z=X6U2T*fKZKSw z@^jW0Bx@Z*yvRTHfrPH&2y$;Y)6O0)?Ysp2Hs!Pr=8Sa!$DNnKT!^&OPH61~_AtIc z-#qY4T1U{X0A_Kkqs+!JAyY3hkCmB2PvTlIh7cP^ocq)<9`-74qnR>82`Q>f_$n6( zI*&xJHfc`Ml2S6k3}!X#V&axjukseqR64Bf?Rk1Fi=B4%-CVfCxBukii#v5MegDkOw!b{Qnf~j>@1M!ky#yIr#?YEGd~=rX$nYIE ztFz(ZOnCT5vFs%&b4l8{6xj*S-Q%PG{U0u_{+vp@?>p6lUGCq^>V-RjBm?9BYo&05 zYo!C}N@?8eWm3X-&4(dcy`O+ZTz@-LO<*jA6l@%Ayi_i4mO$I}Izq!y*zY&Jpx{ zLxjQzVgu7_&DmJY$f#h4^`~e7HdI=XEe&N#Lq9G(e!ueAc4%g&^3CnQn@~BkeDja_ z=B-$^{l!fCi+6eaNK-Xb;g$>!jXc|WKGS;s4nMH({T;sTL9jX-6f!{}8|=vhd$Pfk znc&Ht;OPy|zXc9`*!EG^H@h~!^5?xff#&=F&*j}RZ_vNXd3}DH?U;g%771+U=`L*W ze47uJ$Kp}#9aU4h1jJU~eX2x$A78OpVI{FbBsY6lW{(L->>(DwSmB=nSmzq7$Z)D4 zGMsXbxKl1WGMsXccvGGcK7|OeSTgMvz0)3%pLU48=@QXD?G#I=UA-V_fxBus)k?S+ z;6^Uxy=pnqM7McFr@f0#(UbBHc;k*Z_mb-<7k7ZHX~p0Gg{q6=p^(!==_S~ubP-G* z7KI!fYhN}=%{n-6|1#aROPEV*OzX5z^3hC$C*O-Ku)r3DM=1OmS)vFN;!+fEimMde zfqsjCiGtU8dw0Cx+cYpXfP%4=>W9rQ^+Xhejx>xTt;lQ^dX-fN=SDt+YB$y^t?IOf zIItFn&tm^n^(y!R;L8|KYL}AfM2sDFy>yg0JXj&j8yW)}oV8{LyYa{rhF8hqMg}@P zhIhFiZ}bjx9Olt|l1GJ6TO3W+xm_pktN&@>%+CT7KMTB_3B0`<$5U0*zy_Y_SN67Ji6<296AAg7Rqu7S6#pB zb(Pis&#nuVzgCH)zFybC-7Fn+yZ#|C;QFBlz&?$Gge5y^0vlQJU=i$5C)uO059qY% zhip%kWbCy<{TSBh(-e>ofoU0{<({U89Fki@yOiA?Vx`A*;QTOsd+&oqKo4;g(}68X z3vITTpK+!@qYydxsf%a=wwE0-BnM(h#jz~Y)rd49bxk;sx{ zR7s=bqtl~`vp0+k8EoPuaE?GHovaCRbBi2LQwJKvC~FQ&4RCd7wdOzg@4p+h0%<*hd`|B@Ur_AIRS4U>U<*Q1I1?Q+meI z2gn#N#%Dcr7MS3!F?fgb3wehNyrr;pz8j0Tkjuzz^=$!gTVYbnZ8bCFE*8lXDJl&v zF;g<*DR5^MbzYgmo+9;U@_Zx2T|Q~p>*$|O*n#H>T!msTwE730Q~5c*W`h zB8m5;>HOuCOK>Q8mLhZVlI|OetQfEL3f#{bizg=2OR7xTF=GP=anzd44umH33d30? zE0=IXC8?|`7cdUJ#5fj)eNR@lUP0l2WC3R=vprPc3ryvPSAd;IMn*(I`XRG-E3lyl ztylB;FMddvZgOU+Y=cumfNvGEAO5FmQ`b{Jd-G1)q^V!C+Oyk&2;{=XN zuAw*EFpy~&*l8GqOB~+vY(w9jhQ1FjZw%)GjfP7+7ZmhhC>N~G)im5c654WSg$o(s z!p@PwoY1yg#+9}HC+96If9T>GPwZB6KAd}UBEYSl4IIk^j^zSJ8Qci}*JoZgyy<=} z*i`g%KC|k~E*3sjNB#j#oen9TKR@ZnNVeW%navtd&mPNNaLzbtMUDI$AU&n>Nx{J4 z$E(W{Rd8pyZND(Wu~rax#$6b6EW`=J#bo-IpNQpr7My45$@L69xt`n;GjSbEl0xBP z?k@^6D(s-RmLn^W!E_LG?~253O6lHJ6Ijaw9yN-KODd@;1>`PKtJcTK-MEEP=u2bU zPA3Oqsjv5&_xeA9+^^EhZY8*Ix2+itqE9V3~Jk-NbWvK1>n^lnz|1deWb?grZK zqgeYPzuB@AIJR~8ZlIlB{5JKr@@vzaDUOI4gQA`-qJpIh(jxGt(@+^l(GW${(NoXE zzW~Y7UfrJBha8ey2%{kT5wuri#t`WonooX+zP&%gn4nUJ>w?Lg zQw8L1nTpU2E$z`93I~4Qj>2WccggolI-PP2FR(Hc^3@xP#X$3yN_C%{z(rSF{ge$? zN11W0RHRGHs+Olq-~z`^S#ojTPHfo3(^{h@vNOCe(r`X$qg|DHNy(C-wy zwW`APbRmo4Fu0KAm+}S|U|gpAN;Oit2dGS0r${ZmT^a{^)DlxH#r3`*B4G0uaDcUVwMhmEZ%9Dsh4E zQ4Q{WJ$fF&&PPWm>hj1B@Asot_5s!~<8M5=0(Uya%y(3p=*-TY{RELx?pbw;S_pT0MH zdv`WQsO?Dm=FOWoZ{EDGnf-bw=p)cxT)dU-lL`482TpU_h)_BLNRbQ?Md&BT1WVY0 zxU1iFOteIfi~Vj(0-C$ubIfabw~!5_ohVW>Q9L~^TZ$zs;x-cV-o;@|V1CUo@|I?0 z^M(mS`P=%Lf^MYr0T?tL(o&~&BR#0)vT4rpJf(S>vL)HDELJED!njCYCYGQO%QfV) zL`AUNiVHRun@KKWNs1eAPzG?1;sM;tajzl+UdM4+@d57Vc%2fkd`b|M4Ke=_?X+Ij zX8PD+-O}k;*2tPxHWkff4PB$rVO!UzHL6+Bu`DWT!a$Fv3Y6-G71fQ2E+e1Tqv^Q|EZWjDlBZ*uWunNf?665waeWe0HME>+X2;ZFnoVc)b!lbo&G-l$R4tFX$ER38 zRm&dIRh6~W*rB_2CJ0-a4P5j4q*d}QBbeFFyAqHIKPw{G>fnQO)s;?xszs9ILG1Pe zo8@;mPyFCF&wXhxu<0!z1Z0j}`sikd@M7%@CQ0Uk>r(jfFYelO;M@Q4sb?>rd2X&M z)M1&+e0Hi&Vxc<@ZwAG^F`zU4`v1s57NobP?B z_ivE|i@d-h{VZ~lMUpHsuoUzLLn{%|(6Z9tYsB^PN)u^pvbp!O$WtuR$s#>0vKNpC zxZjXzJ*e`(6-0{VgyMN(0u4cD7eHr9b+8g8g==M;Y|ZLpnCigKRy zi@zc&?l`3NtVHp$X5MlbOoyFI^CccHFH=8E(*S};S)~h@OpM7Qxli0uXc@r=;U(;@ zfN`L+wXH3PXcKOn7x==Ya|$vj^px=-vG*0Yydb38NU~C*6=6Nz0NL$w*1$C%6ZYFeujEu)tq0f6a&yIDJtU1kbncbcpPAV;?{8bE-+80MFJ039Wdh)$3R+0}uRy#* z_ny>#bE&+!|Y9d6C?f1GnVO*X7MK znR&Tmp=HxMz6-vK`+f~ot@oV!jo^dc!83wY9{m`DijKL+Gj7_SgBPKbgOBpoK0KLI zk6hAj^v~{@R^C+=0w&%-8QV=?0D`Z>giT)sfm(4+b)vIkc!KL{Mh*{9D=u?rgw(F< ztdc>7Q3;XtcT8YGUel6jCf=M>PoRa?w4U;K8=<&m%uBaJN81tXK!8EPJerA586GCg zW98B84^(BZmY&eyM}jr13K5&gjei7WL2xu}IzRf;(Tk0jTHkHG(bV~Y^~=fG$t&CE zx<2f>5kGpP=_p=?x5Eu@o_+1?t#JGGaQpi^J_+x<6^>sI$3F@0x+d@9{))M6Wh;7e zwFhE;youq(kMC!ns*dH;g&gAWvOZJLa^;z@s*Yr-Y2n|5k%wuos;2WPRi!)e+*o+* zOnDlyX92JcDt{MbQ>tcJG&@|dbQAc|tXv`c0`%e#0Xo1$`@Kp&bu0JW@Ib%!6x@OL zJW%BC1)w_Imz_AY*W;>K9V$IifL{$znqnM+$*0D%MuuW2G07;5jZfJL+`i85Mz`~8 z0f?#dYRuJHqbHM}u`P3e9tW`)VJ1G4mPA1i{z|sqA+4X2`a9(CeNSB2T_OOulK%l< Ci3TMA diff --git a/src/external_sim_bridge/launch/__pycache__/bridge_sim.launch.cpython-312.pyc b/src/external_sim_bridge/launch/__pycache__/bridge_sim.launch.cpython-312.pyc deleted file mode 100644 index 590f1ee35f6d61017b6f9e7f4dff6f06d1d0d6fc..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 1413 zcmZuxOK;>v5N?k>wlj}pvYtnVY&O=SNNb2PSwKR&T37)AA+gDY+i;3LI68E)bTN+h3ymOrU4BTFc<~$SrqPjZpv`E zU)hTYb;{K8sSZ+xcRB;{5+zcOf*=y95cym=6IK`}^R_Bci8g1G=KOTBKx%WgN>WCE zn03;Cx0zInn1bc3WSqmQZ@}~$j4b{jyVCj6SsCL*o{Qfi%kVVM@0=+G!-lAW%wFd#vX5su=9e3#7>UJl*T*q@r3{%+Hpe1F|Ry&c)Y!r1|!PMXx|(a47p9jr-#%@ z6KX!#Ha{{w=w9e0)Z$Bl7us<;63qwCm`e|jKimalkOpRW8;ynsHo*xt=eC+7x*suW zVsd~(#|vF<(8GkX5c_uQ1$L1EuNSi&!VTdn@K73BGSi`UtRciA2&>AqG1LmXl-G(> z@yMrK32^{kQJRd?gxAKH!5TPX-X~t1K+bR`V#he4z7Gcy#9VhnC#SO%W(bD15FH`9 zf=gkdiA)tjG@FTg1R1pEyJchM^5$)M^QG~>VhNBfiwJ!Xv4~}g@%qW&c@)sWxA7om z&fpDI8Tk1}Hz;cZS#K4{*7%68x-_KlX;Hht?75D!dqC0?Y`=gwMVGzJOz&o0GiyJc zY08TFN@>p;sPj&y8(H_Qto;R`9rbhUYH`540erEn z0CvPrCC;0~+a)OOUE(EaVG@<*cgpIpDBim9$$KhF^b=z1FtIfduk|NEL{guA%r;;t h{tNL6h-+Drq`&VYY2ylQU!f1K(A!gWQ_^zJe*rTfh^qhq diff --git a/src/external_sim_bridge/package.xml b/src/external_sim_bridge/package.xml index 8263b46..44e6e2d 100644 --- a/src/external_sim_bridge/package.xml +++ b/src/external_sim_bridge/package.xml @@ -11,8 +11,6 @@ Dylan Long Apache-2.0 - ament_python - distributed_satellite_sim rclpy std_msgs From 6e2384c63fc0b3993852904ec28582174f526252 Mon Sep 17 00:00:00 2001 From: Dylan Date: Tue, 21 Apr 2026 11:03:56 -0400 Subject: [PATCH 6/7] fix/undervalidation-and-test-drifting --- .../validate-external-sim-bridge.yml | 62 +++++++ src/DistributedSatelliteSim/CMakeLists.txt | 5 + src/external_sim_bridge/doc/design_note.md | 43 +++-- src/external_sim_bridge/setup.py | 6 +- .../test/data/dlqr_reference_trajectory.csv | 91 ---------- .../test/test_basilisk_backend.py | 56 ++++++ .../test/test_bridge_basilisk_launch.py | 164 ++++++++++++++++++ .../test/test_bridge_reference_launch.py | 8 +- 8 files changed, 321 insertions(+), 114 deletions(-) create mode 100644 .github/workflows/validate-external-sim-bridge.yml delete mode 100644 src/external_sim_bridge/test/data/dlqr_reference_trajectory.csv create mode 100644 src/external_sim_bridge/test/test_basilisk_backend.py create mode 100644 src/external_sim_bridge/test/test_bridge_basilisk_launch.py 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/src/DistributedSatelliteSim/CMakeLists.txt b/src/DistributedSatelliteSim/CMakeLists.txt index 197f84b..47d1dcf 100755 --- a/src/DistributedSatelliteSim/CMakeLists.txt +++ b/src/DistributedSatelliteSim/CMakeLists.txt @@ -62,6 +62,11 @@ install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch ) +install(FILES + test/data/dlqr_reference_trajectory.csv + DESTINATION share/${PROJECT_NAME}/test/data +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/src/external_sim_bridge/doc/design_note.md b/src/external_sim_bridge/doc/design_note.md index 1ec2391..9bf0310 100644 --- a/src/external_sim_bridge/doc/design_note.md +++ b/src/external_sim_bridge/doc/design_note.md @@ -127,8 +127,9 @@ That backend should use: 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 existing DLQR reference fixture in - `src/DistributedSatelliteSim/test/data/dlqr_reference_trajectory.csv` +- 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 @@ -221,11 +222,10 @@ documentation: ## Current Basilisk Backend Provenance -The current `basilisk` backend is implemented against Basilisk's published Python API -documentation, because Basilisk is not installed in the active development environment used for -this slice. +The current `basilisk` backend is pinned to the Basilisk Python runtime package +`bsk==2.10.0` for automated validation. -Documentation used while implementing the backend: +Documentation and runtime references used while implementing the backend: - `SimulationBaseClass` API from Basilisk `2.4.0` - `thrusterDynamicEffector` API from Basilisk `2.4.0` @@ -233,10 +233,7 @@ Documentation used while implementing the backend: - `SCStatesMsgPayload` message fields from Basilisk `2.4.0` - `THRArrayOnTimeCmdMsgPayload` message fields from Basilisk `2.4.0` - `spacecraft` module API from Basilisk `2.10.0` - -This is sufficient to implement the backend boundary and fail-fast import behavior, but it is not -yet a substitute for validating against a single installed Basilisk release in CI or in the -development container. +- validated CI runtime pin: `bsk==2.10.0` ## Current Basilisk Scenario Assumptions @@ -255,7 +252,7 @@ Current scenario assumptions are: - each axis command is converted into Basilisk thruster on-time requests over the bridge's `100 ms` timer period -## Current Runtime Requirements and Blockers +## Validated Runtime Record The current backend requires: @@ -264,9 +261,23 @@ The current backend requires: `external_sim_bridge` - launch via `external_sim_bridge/launch/bridge_sim.launch.py` or direct `ros2 run` -Known current blockers and gaps: +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 active workspace used for implementation does not have Basilisk installed -- the backend therefore has startup validation only, not a live end-to-end Basilisk smoke test -- the exact Basilisk release to support still needs to be pinned and tested as a single runtime - source of truth +- 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/setup.py b/src/external_sim_bridge/setup.py index cbff7e1..7f0f69b 100644 --- a/src/external_sim_bridge/setup.py +++ b/src/external_sim_bridge/setup.py @@ -19,12 +19,10 @@ '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', ], ), - ( - f'share/{package_name}/test/data', - ['test/data/dlqr_reference_trajectory.csv'], - ), ], install_requires=['setuptools'], zip_safe=True, diff --git a/src/external_sim_bridge/test/data/dlqr_reference_trajectory.csv b/src/external_sim_bridge/test/data/dlqr_reference_trajectory.csv deleted file mode 100644 index e33bbf4..0000000 --- a/src/external_sim_bridge/test/data/dlqr_reference_trajectory.csv +++ /dev/null @@ -1,91 +0,0 @@ -21.474377061136369,2.292875120797660,21.081978135884185,-0.001232341729441,-0.050158638812040,-0.002059582195653 -16.235280829810684,-12.626303436833686,17.897073550814067,-0.026227831720655,-0.029245513266003,-0.015377821951063 -6.486265440934633,-17.928792370983569,11.604173281188201,-0.026246256566643,-0.000210193261359,-0.019075908867384 --0.921495986127911,-14.657820888936183,5.126211415129908,-0.014339968125821,0.016724520136396,-0.016391037186507 --3.849430758681117,-7.964165980281448,0.167265629767840,-0.002345851112268,0.018792486112254,-0.010759271242311 --3.380891646081291,-2.214866308767135,-2.687034547248812,0.004150239405856,0.012243603636776,-0.004868068419918 --1.591165474919844,0.791424514717339,-3.632177709694075,0.005122266877151,0.004322674674491,-0.000306605690358 --0.043614681468529,1.374181850224460,-3.257640824468955,0.003134742848019,-0.000808406882247,0.002357201198349 -0.664617082282504,0.729638777597417,-2.228566701235212,0.000743898443748,-0.002439505804749,0.003276996280556 -0.657906206414624,-0.073743443877466,-1.086264874954650,-0.000696980811848,-0.001823112190483,0.002977124704970 -0.330971721040675,-0.508581945651428,-0.166113575865198,-0.001020720543594,-0.000547582318208,0.002060718548973 -0.015329011059273,-0.530645138707978,0.397375313967736,-0.000680298454477,0.000377611359071,0.001024392242183 --0.143237538571929,-0.329236505905397,0.617328280461929,-0.000197655023774,0.000674126056910,0.000179853809020 --0.152863852920315,-0.105741122595624,0.587311293554424,0.000120831878710,0.000523171378039,-0.000344197298124 --0.088233598208714,0.031500529500849,0.423053791634292,0.000212536717138,0.000226513992327,-0.000555115183179 --0.019092823861825,0.070106967906883,0.223842397382815,0.000156163687870,-0.000004184846688,-0.000535570424320 -0.020151822277853,0.048841001418285,0.054748201126866,0.000057963862955,-0.000100286848925,-0.000390223036746 -0.027637660377120,0.012405480178353,-0.054875140357848,-0.000013439959627,-0.000092191013124,-0.000209966531991 -0.017362253957798,-0.012388246937803,-0.103289119715632,-0.000039136555874,-0.000041974125598,-0.000055100788509 -0.004024606637649,-0.019278778224219,-0.104852444897458,-0.000031964748966,0.000002694850871,0.000046541557838 --0.004266674868684,-0.014181831153614,-0.079453435073491,-0.000013271501684,0.000023018822655,0.000092518431997 --0.006230665384917,-0.005674057243120,-0.045130586172460,0.000001765353938,0.000022152975767,0.000095399721206 --0.004303784126565,0.000578215803262,-0.014351205462328,0.000007952564771,0.000011720416460,0.000073117886306 --0.001463175685113,0.002967409253729,0.006702437549975,0.000007115515519,0.000001669424568,0.000042151149912 -0.000480705384147,0.002534796430046,0.016962350857774,0.000003425527393,-0.000003559064543,0.000014022041491 -0.001112578343366,0.001061489667573,0.018528740408490,0.000000152452438,-0.000004170256100,-0.000005446033351 -0.000855998414737,-0.000162854126861,0.014772089186698,-0.000001395657131,-0.000002416107405,-0.000015121692885 -0.000320885002231,-0.000676263216993,0.008938341191861,-0.000001430148236,-0.000000431363051,-0.000016818171799 --0.000084269762665,-0.000610022086421,0.003389079963565,-0.000000760233584,0.000000706095743,-0.000013564123234 --0.000235393091205,-0.000302536431028,-0.000607687484002,-0.000000087671257,0.000000908515668,-0.000008318243224 --0.000196770752819,-0.000025539625252,-0.002722033057431,0.000000265986983,0.000000581114914,-0.000003257833008 --0.000088227219720,0.000109222545469,-0.003238751759931,0.000000304924340,0.000000162142484,0.000000428789750 -0.000001842616754,0.000116751578098,-0.002719870958324,0.000000180314705,-0.000000102964871,0.000002412091037 -0.000041380523015,0.000063424998307,-0.001743749380732,0.000000039055105,-0.000000173626043,0.000002932190398 -0.000039395852415,0.000008031988677,-0.000753018135335,-0.000000043459497,-0.000000122624223,0.000002492079204 -0.000019523911640,-0.000021750412277,-0.000002369227561,-0.000000060305606,-0.000000040488042,0.000001617735714 -0.000000974234777,-0.000025489667143,0.000423940411196,-0.000000039256040,0.000000016783630,0.000000716316457 --0.000008122836629,-0.000015385146913,0.000559435231812,-0.000000010888854,0.000000035402972,0.000000025519878 --0.000008538715507,-0.000003624410311,0.000496032772930,0.000000007362721,0.000000027393249,-0.000000372649367 --0.000004748239115,0.000003391380662,0.000335724111448,0.000000012314101,0.000000010893977,-0.000000505043149 --0.000000801535449,0.000004950656525,0.000160645034296,0.000000008806165,-0.000000001742916,-0.000000453517632 -0.000001364282241,0.000003293209478,0.000021080047797,0.000000003064056,-0.000000006665637,-0.000000310602928 -0.000001698521967,0.000000957630118,-0.000063356065400,-0.000000000998119,-0.000000005744219,-0.000000151686158 -0.000001040568890,-0.000000577239587,-0.000095359510000,-0.000000002377098,-0.000000002590837,-0.000000023533219 -0.000000242067852,-0.000001015155690,-0.000089603810618,-0.000000001878888,0.000000000088603,0.000000055045766 --0.000000241501604,-0.000000740604133,-0.000063876663961,-0.000000000758473,0.000000001280685,0.000000085810778 --0.000000351003668,-0.000000267579503,-0.000033272683170,0.000000000115983,0.000000001225477,0.000000081746512 --0.000000236583113,0.000000073921247,-0.000007575060745,0.000000000463499,0.000000000623364,0.000000058948387 --0.000000073150011,0.000000194428025,0.000008897841463,0.000000000404719,0.000000000054302,0.000000031241020 -0.000000035806373,0.000000157127238,0.000016009509005,0.000000000187164,-0.000000000231238,0.000000007695475 -0.000000068497989,0.000000065392334,0.000016029302559,-0.000000000000572,-0.000000000252262,-0.000000007587105 -0.000000050912271,-0.000000007781194,0.000012022117856,-0.000000000086073,-0.000000000142353,-0.000000014352296 -0.000000018673079,-0.000000037776737,0.000006735679242,-0.000000000084417,-0.000000000024519,-0.000000014591041 --0.000000004998987,-0.000000033755994,0.000002049002385,-0.000000000043666,0.000000000041183,-0.000000011068591 --0.000000013529978,-0.000000016014526,-0.000001122621297,-0.000000000004297,0.000000000051900,-0.000000006296100 --0.000000011073868,-0.000000000349568,-0.000002639913351,0.000000000015811,0.000000000032328,-0.000000002011098 --0.000000004733099,0.000000006973609,-0.000002838611662,0.000000000017576,0.000000000008110,0.000000000923222 -0.000000000397924,0.000000007021436,-0.000002239683610,0.000000000010093,-0.000000000006803,0.000000002355919 -0.000000002554019,0.000000003717763,-0.000001338543713,0.000000000001904,-0.000000000010411,0.000000002577836 -0.000000002330667,0.000000000442556,-0.000000491918470,-0.000000000002743,-0.000000000007138,0.000000002057451 -0.000000001124219,-0.000000001270033,0.000000111548094,-0.000000000003572,-0.000000000002261,0.000000001246539 -0.000000000038473,-0.000000001456203,0.000000425828069,-0.000000000002262,0.000000000001044,0.000000000474149 --0.000000000478228,-0.000000000854325,0.000000497314260,-0.000000000000591,0.000000000002067,-0.000000000082761 --0.000000000489611,-0.000000000175402,0.000000413183772,0.000000000000455,0.000000000001559,-0.000000000377855 --0.000000000263834,0.000000000218211,0.000000261889161,0.000000000000719,0.000000000000590,-0.000000000450486 --0.000000000035937,0.000000000295222,0.000000110432020,0.000000000000501,-0.000000000000132,-0.000000000378744 -0.000000000085322,0.000000000190856,-0.000000003148275,0.000000000000164,-0.000000000000401,-0.000000000243110 -0.000000000100470,0.000000000052757,-0.000000066773849,-0.000000000000067,-0.000000000000334,-0.000000000105241 -0.000000000059873,-0.000000000035503,-0.000000086118198,-0.000000000000142,-0.000000000000146,-0.000000000000669 -0.000000000012941,-0.000000000059196,-0.000000075499862,-0.000000000000109,0.000000000000010,0.000000000058805 --0.000000000014701,-0.000000000042095,-0.000000050550707,-0.000000000000042,0.000000000000076,0.000000000077792 --0.000000000020433,-0.000000000014377,-0.000000023728653,0.000000000000008,0.000000000000071,0.000000000069059 --0.000000000013411,0.000000000005112,-0.000000002568483,0.000000000000027,0.000000000000035,0.000000000046793 --0.000000000003861,0.000000000011622,0.000000010075986,0.000000000000023,0.000000000000002,0.000000000022435 -0.000000000002337,0.000000000009110,0.000000014721082,0.000000000000010,-0.000000000000014,0.000000000002997 -0.000000000004070,0.000000000003652,0.000000013665007,-0.000000000000000,-0.000000000000015,-0.000000000008779 -0.000000000002940,-0.000000000000575,0.000000009640373,-0.000000000000005,-0.000000000000008,-0.000000000013256 -0.000000000001032,-0.000000000002241,0.000000004941122,-0.000000000000005,-0.000000000000001,-0.000000000012472 --0.000000000000331,-0.000000000001946,0.000000001037337,-0.000000000000002,0.000000000000003,-0.000000000008901 --0.000000000000799,-0.000000000000891,-0.000000001436669,-0.000000000000000,0.000000000000003,-0.000000000004644 --0.000000000000636,0.000000000000013,-0.000000002479592,0.000000000000001,0.000000000000002,-0.000000000001066 --0.000000000000261,0.000000000000421,-0.000000002449452,0.000000000000001,0.000000000000000,0.000000000001231 -0.000000000000034,0.000000000000409,-0.000000001818302,0.000000000000001,-0.000000000000000,0.000000000002225 -0.000000000000153,0.000000000000210,-0.000000001004525,0.000000000000000,-0.000000000000001,0.000000000002231 -0.000000000000135,0.000000000000020,-0.000000000291181,-0.000000000000000,-0.000000000000000,0.000000000001675 -0.000000000000063,-0.000000000000077,0.000000000186374,-0.000000000000000,-0.000000000000000,0.000000000000940 -0.000000000000000,-0.000000000000085,0.000000000410488,-0.000000000000000,0.000000000000000,0.000000000000287 --0.000000000000029,-0.000000000000048,0.000000000434684,-0.000000000000000,0.000000000000000,-0.000000000000155 --0.000000000000028,-0.000000000000009,0.000000000339434,0.000000000000000,0.000000000000000,-0.000000000000367 --0.000000000000015,0.000000000000013,0.000000000200322,0.000000000000000,0.000000000000000,-0.000000000000395 --0.000000000000002,0.000000000000017,0.000000000071209,0.000000000000000,-0.000000000000000,-0.000000000000312 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..3f82a92 --- /dev/null +++ b/src/external_sim_bridge/test/test_bridge_basilisk_launch.py @@ -0,0 +1,164 @@ +#!/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 + + +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: + + @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() + + @classmethod + def tearDownClass(cls): + cls.recorder.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + def test_basilisk_bridge_publishes_finite_state_trajectory(self): + 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}', + ) + + saw_motion = False + previous_state = None + for step, state in enumerate(self.recorder.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 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') + + + @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_reference_launch.py b/src/external_sim_bridge/test/test_bridge_reference_launch.py index 746c076..da830ea 100644 --- a/src/external_sim_bridge/test/test_bridge_reference_launch.py +++ b/src/external_sim_bridge/test/test_bridge_reference_launch.py @@ -22,7 +22,7 @@ from std_msgs.msg import Float64MultiArray REFERENCE_FIXTURE_PATH = os.path.join( - get_package_share_directory('external_sim_bridge'), + get_package_share_directory('distributed_satellite_sim'), 'test', 'data', 'dlqr_reference_trajectory.csv', @@ -99,13 +99,15 @@ def generate_test_description(): class TestBridgeReferenceTrajectory(unittest.TestCase): @classmethod def setUpClass(cls): - rclpy.init() + if not rclpy.ok(): + rclpy.init() cls.recorder = TrajectoryRecorder() @classmethod def tearDownClass(cls): cls.recorder.destroy_node() - rclpy.shutdown() + if rclpy.ok(): + rclpy.shutdown() def test_bridge_ros_trajectory_matches_reference(self): reference_trajectory = _load_reference_trajectory() From 7b2209ff252480d51d3ebfab962aec5fcd3c44c3 Mon Sep 17 00:00:00 2001 From: Dylan Date: Tue, 21 Apr 2026 13:28:10 -0400 Subject: [PATCH 7/7] fix: basilisk backend timestamps --- .../backends/basilisk_backend.py | 12 +++++- .../test/test_bridge_basilisk_launch.py | 37 ++++++++++++++++++- 2 files changed, 45 insertions(+), 4 deletions(-) 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 index b995202..344a5ba 100644 --- a/src/external_sim_bridge/external_sim_bridge/backends/basilisk_backend.py +++ b/src/external_sim_bridge/external_sim_bridge/backends/basilisk_backend.py @@ -101,7 +101,13 @@ def validate_control(self, control: Sequence[float]) -> None: def apply_control(self, control: Sequence[float]) -> None: self.validate_control(control) self._control = [float(value) for value in control] - self._thruster_command_msg.write(self._build_thruster_payload(self._control_to_on_times())) + # 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: @@ -196,6 +202,8 @@ def _control_to_on_times(self) -> list[float]: 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): - payload.OnTimeRequest[index] = float(on_time) + requested_on_times[index] = float(on_time) + payload.OnTimeRequest = requested_on_times return payload diff --git a/src/external_sim_bridge/test/test_bridge_basilisk_launch.py b/src/external_sim_bridge/test/test_bridge_basilisk_launch.py index 3f82a92..857068e 100644 --- a/src/external_sim_bridge/test/test_bridge_basilisk_launch.py +++ b/src/external_sim_bridge/test/test_bridge_basilisk_launch.py @@ -46,6 +46,7 @@ SHUTDOWN_AFTER_SEC = 15.0 RECEIVE_TIMEOUT_SEC = 12.0 DISCOVERY_TIMEOUT_SEC = 10.0 +CONTROL_EFFECT_TOLERANCE = 1e-6 class BasiliskTrajectoryRecorder(Node): @@ -64,6 +65,7 @@ def _callback(self, msg): if BASILISK_AVAILABLE: + from external_sim_bridge.backends.basilisk_backend import BasiliskBackend @pytest.mark.rostest def generate_test_description(): @@ -98,6 +100,7 @@ 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): @@ -105,7 +108,18 @@ def tearDownClass(cls): if rclpy.ok(): rclpy.shutdown() - def test_basilisk_bridge_publishes_finite_state_trajectory(self): + @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 @@ -133,9 +147,17 @@ def test_basilisk_bridge_publishes_finite_state_trajectory(self): 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 in enumerate(self.recorder.trajectory): + for step, (state, baseline_state) in enumerate( + zip(self.recorder.trajectory, self.zero_control_trajectory) + ): self.assertEqual( len(state), STATE_SIZE, @@ -147,6 +169,12 @@ def test_basilisk_bridge_publishes_finite_state_trajectory(self): 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) @@ -156,6 +184,11 @@ def test_basilisk_bridge_publishes_finite_state_trajectory(self): 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()