diff --git a/.ci/gazebo.Dockerfile b/.ci/gazebo.Dockerfile index 2457d2c..1798541 100644 --- a/.ci/gazebo.Dockerfile +++ b/.ci/gazebo.Dockerfile @@ -1,12 +1,93 @@ -# .ci/gazebo.Dockerfile — SAKURA-II Gazebo Harmonic headless SITL image. +# .ci/gazebo.Dockerfile — SAKURA-II Gazebo Harmonic SITL image (GUI-capable). # -# Builds from ubuntu:22.04 + OSRF apt repository because -# ghcr.io/gazebosim/gz-sim:harmonic requires authentication. -# Runs gz sim in headless server mode (-s flag); no display required. -# Simulation world SDF and plugins are mounted read-only from ./simulation. +# Two-stage build: +# Stage 1 (builder): installs Gazebo dev headers, compiles all 4 system +# plugins into shared libraries. +# Stage 2 (runtime): installs Gazebo runtime + X11/Mesa GUI packages, copies +# .so files from builder. Final image is ~400 MB smaller than single-stage. +# +# GUI support: the runtime stage includes Mesa llvmpipe (software renderer) and +# X11 client libraries so gz sim can open a window when $DISPLAY is forwarded +# from the host via /tmp/.X11-unix (see compose.yaml gazebo service). +# LIBGL_ALWAYS_SOFTWARE=1 forces Mesa llvmpipe; no GPU passthrough required. +# +# Plugin discovery: GZ_SIM_SYSTEM_PLUGIN_PATH=/usr/local/lib (Harmonic API). +# World file discovery: GZ_SIM_RESOURCE_PATH=/app/simulation/worlds. # No HPSC cross-toolchain (Q-H8 deferred to Phase C+). # No secrets embedded. +# ── Stage 1: builder ────────────────────────────────────────────────────────── +FROM ubuntu:22.04 AS builder + +ENV DEBIAN_FRONTEND=noninteractive + +RUN apt-get update -q && \ + apt-get install -y --no-install-recommends \ + curl \ + gnupg \ + lsb-release \ + ca-certificates \ + && rm -rf /var/lib/apt/lists/* + +# Add OSRF Gazebo Harmonic apt repository. +RUN curl -fsSL https://packages.osrfoundation.org/gazebo.gpg \ + -o /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg && \ + echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" \ + > /etc/apt/sources.list.d/gazebo-stable.list && \ + apt-get update -q && \ + apt-get install -y --no-install-recommends \ + gz-harmonic \ + libgz-sim8-dev \ + libgz-transport13-dev \ + libgz-plugin2-dev \ + libgz-math7-dev \ + cmake \ + g++ \ + make \ + ca-certificates \ + && rm -rf /var/lib/apt/lists/* + +# Copy plugin source trees into the builder. +COPY simulation/gazebo_rover_plugin /src/gazebo_rover_plugin +COPY simulation/gazebo_uav_plugin /src/gazebo_uav_plugin +COPY simulation/gazebo_cryobot_plugin /src/gazebo_cryobot_plugin +COPY simulation/gazebo_world_plugin /src/gazebo_world_plugin + +# Build and install each plugin. Unit-test targets (uav_plugin_test, etc.) +# have no Gazebo dependency and are skipped here (BUILD_TESTING=OFF). +RUN cmake -B /build/rover \ + -DCMAKE_BUILD_TYPE=Release \ + -DCMAKE_INSTALL_PREFIX=/usr/local \ + -DBUILD_TESTING=OFF \ + /src/gazebo_rover_plugin && \ + cmake --build /build/rover && \ + cmake --install /build/rover + +RUN cmake -B /build/uav \ + -DCMAKE_BUILD_TYPE=Release \ + -DCMAKE_INSTALL_PREFIX=/usr/local \ + -DBUILD_TESTING=OFF \ + /src/gazebo_uav_plugin && \ + cmake --build /build/uav && \ + cmake --install /build/uav + +RUN cmake -B /build/cryobot \ + -DCMAKE_BUILD_TYPE=Release \ + -DCMAKE_INSTALL_PREFIX=/usr/local \ + -DBUILD_TESTING=OFF \ + /src/gazebo_cryobot_plugin && \ + cmake --build /build/cryobot && \ + cmake --install /build/cryobot + +RUN cmake -B /build/world \ + -DCMAKE_BUILD_TYPE=Release \ + -DCMAKE_INSTALL_PREFIX=/usr/local \ + -DBUILD_TESTING=OFF \ + /src/gazebo_world_plugin && \ + cmake --build /build/world && \ + cmake --install /build/world + +# ── Stage 2: runtime ───────────────────────────────────────────────────────── FROM ubuntu:22.04 ENV DEBIAN_FRONTEND=noninteractive @@ -20,10 +101,7 @@ RUN apt-get update -q && \ procps \ && rm -rf /var/lib/apt/lists/* -# Add OSRF Gazebo Harmonic apt repository. -# NOTE: the echo must be a single line — a multi-line echo with indented -# continuation puts leading spaces into the URI, which APT silently ignores, -# making gz-harmonic unlocalizable. +# Add OSRF Gazebo Harmonic apt repository (runtime packages only). RUN curl -fsSL https://packages.osrfoundation.org/gazebo.gpg \ -o /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg && \ echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" \ @@ -31,11 +109,32 @@ RUN curl -fsSL https://packages.osrfoundation.org/gazebo.gpg \ apt-get update -q && \ apt-get install -y --no-install-recommends \ gz-harmonic \ + # Mesa llvmpipe software renderer — GPU-free OpenGL for Docker containers. + libgl1-mesa-dri \ + libgl1 \ + libegl1 \ + libglu1-mesa \ + # X11 client libraries for forwarding the GUI window to the host display. + libx11-6 \ + libx11-xcb1 \ + libxrender1 \ + libxext6 \ + libxcb1 \ && rm -rf /var/lib/apt/lists/* +# Copy compiled plugin .so files from the builder stage. +COPY --from=builder /usr/local/lib/librover_drive_plugin.so /usr/local/lib/ +COPY --from=builder /usr/local/lib/libuav_flight_plugin.so /usr/local/lib/ +COPY --from=builder /usr/local/lib/libcryobot_physics_plugin.so /usr/local/lib/ +COPY --from=builder /usr/local/lib/libworld_environment_plugin.so /usr/local/lib/ + +RUN ldconfig + RUN mkdir -p /app/simulation -# Plugin and world file discovery path for gz sim. +# GZ_SIM_RESOURCE_PATH: world .sdf and mesh files mounted from ./simulation +# GZ_SIM_SYSTEM_PLUGIN_PATH: where gz-sim scans for compiled system plugins ENV GZ_SIM_RESOURCE_PATH=/app/simulation/worlds +ENV GZ_SIM_SYSTEM_PLUGIN_PATH=/usr/local/lib WORKDIR /app diff --git a/.ci/ros2.Dockerfile b/.ci/ros2.Dockerfile index 73c2885..60a4521 100644 --- a/.ci/ros2.Dockerfile +++ b/.ci/ros2.Dockerfile @@ -7,6 +7,20 @@ FROM osrf/space-ros:latest # log/ and install/ outputs there without a permission error. USER root RUN mkdir -p /workspace && chown spaceros-user:spaceros-user /workspace + +# Install ros_gz_bridge and ros_gz_sim for Gazebo Harmonic ↔ ROS 2 topic bridging. +# The || true allows the build to proceed if the Space ROS apt overlay does not +# carry these packages — the colcon build in compose.yaml then builds them from +# source via the ros_gz_src clone below. +RUN apt-get update -q && \ + apt-get install -y --no-install-recommends \ + ros-${ROS_DISTRO}-ros-gz-bridge \ + ros-${ROS_DISTRO}-ros-gz-sim \ + && rm -rf /var/lib/apt/lists/* \ + || (rm -rf /var/lib/apt/lists/* && \ + git clone --depth 1 -b harmonic \ + https://github.com/gazebosim/ros_gz /workspace/ros_gz_src || true) + USER spaceros-user WORKDIR /workspace diff --git a/CMakeLists.txt b/CMakeLists.txt index d93b789..fe80f3c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -54,6 +54,7 @@ add_subdirectory(apps/orbiter_payload) add_subdirectory(apps/mcu_payload_gw) add_subdirectory(apps/mcu_rwa_gw) add_subdirectory(apps/mcu_eps_gw) +add_subdirectory(apps/sim_adapter) # ── Simulation Plugins ──────────────────────────────────────────────────────── # Gazebo plugins require Gazebo to be installed. Each subdirectory guards diff --git a/Makefile b/Makefile index 52ace80..f83a4c4 100644 --- a/Makefile +++ b/Makefile @@ -6,16 +6,31 @@ # make stop stop and remove containers (named volumes are preserved) # make logs stream combined logs from all running services (Ctrl-C to exit) # make clean stop containers AND remove named volumes (destructive) +# +# Gazebo GUI: on a desktop with X11, the Gazebo 3D window opens automatically. +# On a headless server (no DISPLAY), Gazebo runs server-only (no window). +# Requires x11-xserver-utils (xhost) on the host: apt install x11-xserver-utils .PHONY: run stop logs clean run: + @if [ -n "$$DISPLAY" ]; then \ + echo "X11 display detected ($$DISPLAY) — granting Docker X11 access..."; \ + xhost +local:docker 2>/dev/null || echo " (xhost not found — install x11-xserver-utils if the Gazebo window does not appear)"; \ + else \ + echo "No DISPLAY set — Gazebo will run headless (server-only)."; \ + fi docker compose up --build -d @echo "" @echo "Stack is starting. Current service status:" @docker compose ps @echo "" - @echo " Ground station UI → http://localhost:8080/api/time" + @if [ -n "$$DISPLAY" ]; then \ + echo " Gazebo 3D view → opens automatically (X11 forwarded)"; \ + else \ + echo " Gazebo → headless (no DISPLAY set)"; \ + fi + @echo " Ground station API → http://localhost:8080/api/time" @echo " UDP telemetry in → localhost:10000" @echo "" @echo " make logs — stream all service logs" diff --git a/_defs/mids.h b/_defs/mids.h index fc12f00..cf228be 100644 --- a/_defs/mids.h +++ b/_defs/mids.h @@ -51,6 +51,10 @@ /* ── Sim injection block anchor (sideband only; never flight-path) ──────────── */ #define SIM_INJECT_HK_MID 0x0D00U /* 0x0800 | 0x500 */ +/* ── sim_adapter MIDs (SITL only; CFS_FLIGHT_BUILD excluded) ────────────────── */ +#define SIM_ADAPTER_CMD_MID 0x1D00U /* 0x1800 | 0x500 */ +#define SIM_ADAPTER_HK_MID 0x0D01U /* 0x0800 | 0x501 */ + /* ── Fault-injection SPP APIDs (sideband only; ICD-sim-fsw.md §2) ─────────── * These are raw APID values (not cFE MIDs) used by simulation/fault_injector. * They are never placed on the cFE Software Bus (CFS_FLIGHT_BUILD guard diff --git a/_defs/targets.cmake b/_defs/targets.cmake index c154546..a6c0e0b 100644 --- a/_defs/targets.cmake +++ b/_defs/targets.cmake @@ -27,6 +27,7 @@ set(MISSION_APPS mcu_payload_gw mcu_rwa_gw mcu_eps_gw + sim_adapter ) message(STATUS "Mission: ${MISSION_NAME} SCID: ${SPACECRAFT_ID} Apps: ${MISSION_APPS}") diff --git a/apps/sim_adapter/CMakeLists.txt b/apps/sim_adapter/CMakeLists.txt new file mode 100644 index 0000000..3b98360 --- /dev/null +++ b/apps/sim_adapter/CMakeLists.txt @@ -0,0 +1,11 @@ +# apps/sim_adapter/CMakeLists.txt +# +# Build contract provided by sakura_add_cfs_app() (see _defs/cfs_app_template.cmake): +# sim_adapter OBJECT library (FSW source) +# sim_adapter_test CMocka unit-test executable +# sim_adapter_unit_tests CTest entry +# sim_adapter_cppcheck cppcheck development target +# +# CFS_FLIGHT_BUILD guard: the entire sim_adapter implementation is excluded from +# flight builds by #ifndef CFS_FLIGHT_BUILD in sim_adapter.c / sim_adapter.h. +sakura_add_cfs_app(sim_adapter) diff --git a/apps/sim_adapter/fsw/src/cfe.h b/apps/sim_adapter/fsw/src/cfe.h new file mode 100644 index 0000000..7426441 --- /dev/null +++ b/apps/sim_adapter/fsw/src/cfe.h @@ -0,0 +1,102 @@ +#ifndef CFE_H +#define CFE_H + +/* + * cfe.h — Minimal cFE / OSAL type and constant stubs for sim_adapter standalone builds. + * + * Extends orbiter_comm's stub pattern with: + * - CFE_EVS_BinFilter_t (event filter struct for EVS_Register) + * - CFE_MSG_SetMsgId / CFE_MSG_SetSize (message header setters) + * - OS_SocketBind / OS_SocketRecvFrom (OSAL receive-side socket API) + * - OS_ERR_TIMEOUT constant + * + * MISRA C:2012 Rule 20.5 deviation: conditionally compiled; superseded by the + * real cfe.h when cFS is present on the include path ahead of this stub. + */ + +#include +#include +#include +#include + +/* ── Primitive typedefs (mirror cFE common_types.h) ─────────────────────── */ +typedef int32_t int32; +typedef uint16_t uint16; +typedef uint32_t uint32; +typedef uint8_t uint8; + +/* ── Software Bus types ──────────────────────────────────────────────────── */ +typedef uintptr_t CFE_SB_PipeId_t; +typedef uint32_t CFE_SB_MsgId_Atom_t; +typedef CFE_SB_MsgId_Atom_t CFE_SB_MsgId_t; + +/* CCSDS primary-header minimum (6 bytes) plus secondary-header stub (10 bytes) */ +typedef struct { uint8 Byte[16]; } CFE_MSG_Message_t; +typedef struct { CFE_MSG_Message_t Msg; } CFE_SB_Buffer_t; +typedef uint16_t CFE_MSG_FcnCode_t; + +/* ── Event Services types ────────────────────────────────────────────────── */ +typedef struct { + uint16 EventID; + uint16 Mask; +} CFE_EVS_BinFilter_t; + +/* ── Status codes ────────────────────────────────────────────────────────── */ +#define CFE_SUCCESS ((int32) 0) +#define CFE_SB_BAD_ARGUMENT ((int32)-1) +#define CFE_SB_PIPE_RD_ERR ((int32)-2) +#define CFE_SB_MAX_MSGS_MET ((int32)-3) +#define CFE_ES_ERR_APP_REGISTER ((int32)-4) +#define CFE_EVS_APP_FILTER_OVERLOAD ((int32)-5) + +/* ── Executive Services constants ───────────────────────────────────────── */ +#define CFE_ES_RunStatus_APP_RUN 1U +#define CFE_ES_RunStatus_APP_ERROR 2U +#define CFE_ES_RunStatus_APP_EXIT 3U + +/* ── Software Bus constants ─────────────────────────────────────────────── */ +#define CFE_SB_PEND_FOREVER ((int32)-1) +#define CFE_SB_INVALID_MSG_ID ((CFE_SB_MsgId_t)0xFFFFU) + +/* ── Event Services constants ────────────────────────────────────────────── */ +#define CFE_EVS_EventFilter_BINARY 1U +#define CFE_EVS_EventType_INFORMATION 1U +#define CFE_EVS_EventType_ERROR 4U + +/* ── API declarations (implemented by cFE or by UNIT_TEST stubs) ─────────── */ +int32 CFE_ES_RegisterApp(void); +bool CFE_ES_RunLoop(uint32 *RunStatus); +void CFE_ES_ExitApp(uint32 ExitStatus); + +int32 CFE_EVS_Register(const void *Filters, uint16 NumFilters, uint16 FilterScheme); +void CFE_EVS_SendEvent(uint16 EventID, uint16 EventType, const char *Spec, ...); + +int32 CFE_SB_CreatePipe(CFE_SB_PipeId_t *PipeIdPtr, uint16 Depth, const char *PipeName); +int32 CFE_SB_Subscribe(CFE_SB_MsgId_t MsgId, CFE_SB_PipeId_t PipeId); +int32 CFE_SB_ReceiveBuffer(CFE_SB_Buffer_t **BufPtr, CFE_SB_PipeId_t PipeId, int32 TimeOut); +int32 CFE_SB_TransmitMsg(CFE_MSG_Message_t *MsgPtr, bool IncrementSequenceCount); +CFE_SB_MsgId_Atom_t CFE_SB_MsgIdToValue(CFE_SB_MsgId_t MsgId); +CFE_SB_MsgId_t CFE_SB_ValueToMsgId(CFE_SB_MsgId_Atom_t MsgIdValue); + +int32 CFE_MSG_GetMsgId(const CFE_MSG_Message_t *MsgPtr, CFE_SB_MsgId_t *MsgId); +int32 CFE_MSG_GetFcnCode(const CFE_MSG_Message_t *MsgPtr, CFE_MSG_FcnCode_t *FcnCode); +int32 CFE_MSG_SetMsgId(CFE_MSG_Message_t *MsgPtr, CFE_SB_MsgId_t MsgId); +int32 CFE_MSG_SetSize(CFE_MSG_Message_t *MsgPtr, uint32 TotalMsgSize); + +/* ── OSAL network types ──────────────────────────────────────────────────── */ +typedef uint32_t osal_id_t; +typedef struct { uint8_t AddrData[28]; } OS_SockAddr_t; +typedef enum { OS_SocketDomain_INET = 2 } OS_SocketDomain_t; +typedef enum { OS_SocketType_DATAGRAM = 2 } OS_SocketType_t; + +#define OS_SUCCESS ((int32) 0) +#define OS_ERR_TIMEOUT ((int32)-10) + +int32 OS_SocketOpen(osal_id_t *sock_id, OS_SocketDomain_t Domain, OS_SocketType_t Type); +int32 OS_SocketAddrInit(OS_SockAddr_t *Addr, OS_SocketDomain_t Domain); +int32 OS_SocketAddrSetPort(OS_SockAddr_t *Addr, uint16 PortNum); +int32 OS_SocketBind(osal_id_t sock_id, const OS_SockAddr_t *Addr); +int32 OS_SocketRecvFrom(osal_id_t sock_id, void *buffer, uint32 buflen, + OS_SockAddr_t *RemoteAddr, int32 timeout_ms); + +#endif /* CFE_H */ diff --git a/apps/sim_adapter/fsw/src/sim_adapter.c b/apps/sim_adapter/fsw/src/sim_adapter.c new file mode 100644 index 0000000..6e27cf0 --- /dev/null +++ b/apps/sim_adapter/fsw/src/sim_adapter.c @@ -0,0 +1,301 @@ +/* + * sim_adapter.c — SAKURA-II cFS SITL UDP–to–Software-Bus adapter. + * + * Polls a UDP socket on port 5700 for CCSDS Space Packets from the + * fault_injector, validates APID (ICD-sim-fsw.md §4.1) and + * CRC-16/CCITT-FALSE (poly 0x1021, init 0xFFFF), then routes valid + * packets to the cFE Software Bus via SIM_INJECT_HK_MID. + * + * Entire body guarded by #ifndef CFS_FLIGHT_BUILD — zero symbols in + * flight image (ICD-sim-fsw.md §5.2). + * + * MISRA C:2012 compliance target. No dynamic memory allocation. + * Stack depth statically bounded. + */ + +#ifndef CFS_FLIGHT_BUILD + +#include "sim_adapter.h" +#include + +/* ── File-scope state ────────────────────────────────────────────────────── */ +#ifdef UNIT_TEST +/* Non-static so unit tests can inspect counters directly. */ +SIM_ADAPTER_AppData_t SIM_ADAPTER_Data; +#else +static SIM_ADAPTER_AppData_t SIM_ADAPTER_Data; +#endif /* UNIT_TEST */ + +/* MISRA Rule 18.8: fixed-size; not a VLA. */ +static uint8 frame_buf[SIM_ADAPTER_FRAME_BUF_SIZE]; + +/* ── Forward declarations ─────────────────────────────────────────────────── */ +static int32 SIM_ADAPTER_Init(void); +static void SIM_ADAPTER_RouteToSB(uint16 apid, const uint8 *payload, + uint32 payload_len); + +#ifndef UNIT_TEST +static int32 SIM_ADAPTER_ProcessUdp(const uint8 *buf, uint32 len); +static uint16 SIM_ADAPTER_Crc16(const uint8 *data, uint16 len); +#endif /* !UNIT_TEST */ + +/* --------------------------------------------------------------------------- + * SIM_ADAPTER_AppMain — Application entry point + * --------------------------------------------------------------------------- */ +void SIM_ADAPTER_AppMain(void) +{ + int32 status; + + SIM_ADAPTER_Data.RunStatus = CFE_ES_RunStatus_APP_RUN; + + status = SIM_ADAPTER_Init(); + if (status != CFE_SUCCESS) + { + SIM_ADAPTER_Data.RunStatus = CFE_ES_RunStatus_APP_ERROR; + } + + while (CFE_ES_RunLoop(&SIM_ADAPTER_Data.RunStatus) == true) + { + /* Non-blocking poll: returns byte count (>0) or negative error code. */ + int32 bytes_recv = OS_SocketRecvFrom(SIM_ADAPTER_Data.UdpSockId, + frame_buf, SIM_ADAPTER_FRAME_BUF_SIZE, + NULL, 0); + if (bytes_recv > 0) + { + (void)SIM_ADAPTER_ProcessUdp(frame_buf, (uint32)bytes_recv); + } + + SIM_ADAPTER_Data.UptimeSeconds++; + } + + CFE_ES_ExitApp(SIM_ADAPTER_Data.RunStatus); +} + +/* --------------------------------------------------------------------------- + * SIM_ADAPTER_Init — One-time application initialization + * --------------------------------------------------------------------------- */ +static int32 SIM_ADAPTER_Init(void) +{ + int32 status; + + status = CFE_ES_RegisterApp(); + if (status != CFE_SUCCESS) + { + return status; + } + + status = CFE_EVS_Register(SIM_ADAPTER_Data.EventFilters, + (uint16)SIM_ADAPTER_EVT_COUNT, + (uint16)CFE_EVS_EventFilter_BINARY); + if (status != CFE_SUCCESS) + { + return status; + } + + status = CFE_SB_CreatePipe(&SIM_ADAPTER_Data.CmdPipe, + (uint16)SIM_ADAPTER_PIPE_DEPTH, + "SIM_ADAPTER_CMD"); + if (status != CFE_SUCCESS) + { + return status; + } + + status = CFE_SB_Subscribe(CFE_SB_ValueToMsgId(SIM_ADAPTER_CMD_MID), + SIM_ADAPTER_Data.CmdPipe); + if (status != CFE_SUCCESS) + { + return status; + } + + status = OS_SocketOpen(&SIM_ADAPTER_Data.UdpSockId, + OS_SocketDomain_INET, OS_SocketType_DATAGRAM); + if (status != OS_SUCCESS) + { + CFE_EVS_SendEvent(SIM_ADAPTER_EID_INIT_SOCKET_ERR, + CFE_EVS_EventType_ERROR, + "SIM_ADAPTER: UDP socket open failed 0x%08X", + (unsigned int)status); + return status; + } + + status = OS_SocketAddrInit(&SIM_ADAPTER_Data.UdpBindAddr, + OS_SocketDomain_INET); + if (status != OS_SUCCESS) + { + return status; + } + + status = OS_SocketAddrSetPort(&SIM_ADAPTER_Data.UdpBindAddr, + (uint16)SIM_ADAPTER_UDP_PORT); + if (status != OS_SUCCESS) + { + return status; + } + + status = OS_SocketBind(SIM_ADAPTER_Data.UdpSockId, + &SIM_ADAPTER_Data.UdpBindAddr); + if (status != OS_SUCCESS) + { + return status; + } + + SIM_ADAPTER_Data.PacketsRouted = 0U; + SIM_ADAPTER_Data.CrcMismatches = 0U; + SIM_ADAPTER_Data.ApidRejects = 0U; + SIM_ADAPTER_Data.UptimeSeconds = 0U; + + CFE_EVS_SendEvent(SIM_ADAPTER_STARTUP_INF_EID, + CFE_EVS_EventType_INFORMATION, + "SIM_ADAPTER initialized v%d.%d.%d (UDP port %u)", + SIM_ADAPTER_MAJOR_VERSION, + SIM_ADAPTER_MINOR_VERSION, + SIM_ADAPTER_REVISION, + (unsigned int)SIM_ADAPTER_UDP_PORT); + + return CFE_SUCCESS; +} + +/* --------------------------------------------------------------------------- + * SIM_ADAPTER_Crc16 — CRC-16/CCITT-FALSE (poly 0x1021, init 0xFFFF, no reflect) + * + * Identical algorithm to MCU_EPS_GW_Crc16 and spp_crc16() in fault_injector. + * Canonical check: Crc16("123456789", 9) == 0x29B1. + * Per ICD-sim-fsw.md §4.1 and Q-C9. + * --------------------------------------------------------------------------- */ +#ifdef UNIT_TEST +uint16 SIM_ADAPTER_Crc16(const uint8 *data, uint16 len) +#else +static uint16 SIM_ADAPTER_Crc16(const uint8 *data, uint16 len) +#endif /* UNIT_TEST */ +{ + uint16 crc = SIM_ADAPTER_CRC16_INIT; + uint16 i; + uint8 bit; + + for (i = 0U; i < len; i++) + { + crc ^= (uint16)((uint16)data[i] << 8U); + for (bit = 0U; bit < 8U; bit++) + { + if ((crc & 0x8000U) != 0U) + { + crc = (uint16)((uint16)(crc << 1U) ^ SIM_ADAPTER_CRC16_POLY); + } + else + { + crc = (uint16)(crc << 1U); + } + } + } + return crc; +} + +/* --------------------------------------------------------------------------- + * SIM_ADAPTER_ProcessUdp — Validate and dispatch one UDP datagram. + * + * Validation order per ICD-sim-fsw.md §4.1: + * 1. Length gate (< 18 bytes → reject) + * 2. APID extraction (big-endian, bytes 0–1) + * 3. APID range gate (0x500–0x57F) + * 4. CRC-16/CCITT-FALSE over payload bytes (buf[16..len-3]) + * 5. Route valid packet to SB + * --------------------------------------------------------------------------- */ +#ifdef UNIT_TEST +int32 SIM_ADAPTER_ProcessUdp(const uint8 *buf, uint32 len) +#else +static int32 SIM_ADAPTER_ProcessUdp(const uint8 *buf, uint32 len) +#endif /* UNIT_TEST */ +{ + uint16 apid; + uint16 crc_computed; + uint16 crc_stored; + + /* 1. Length gate: need at least 16-byte header + 2-byte CRC = 18 bytes. */ + if (len < 18U) + { + CFE_EVS_SendEvent(SIM_ADAPTER_EID_PACKET_TOO_SHORT, + CFE_EVS_EventType_ERROR, + "SIM_ADAPTER: datagram too short (%u bytes)", + (unsigned int)len); + SIM_ADAPTER_Data.ApidRejects++; + return CFE_SB_BAD_ARGUMENT; + } + + /* 2. APID from CCSDS primary header bytes 0–1 (big-endian, 11-bit field). */ + apid = (uint16)(((uint16)(buf[0U] & 0x07U) << 8U) | (uint16)buf[1U]); + + /* 3. APID range gate: sim sideband block 0x500–0x57F only. */ + if ((apid < SIM_ADAPTER_APID_MIN) || (apid > SIM_ADAPTER_APID_MAX)) + { + CFE_EVS_SendEvent(SIM_ADAPTER_EID_APID_OUT_OF_RANGE, + CFE_EVS_EventType_ERROR, + "SIM_ADAPTER: APID 0x%03X out of range [0x%03X–0x%03X]", + (unsigned int)apid, + (unsigned int)SIM_ADAPTER_APID_MIN, + (unsigned int)SIM_ADAPTER_APID_MAX); + SIM_ADAPTER_Data.ApidRejects++; + return CFE_SB_BAD_ARGUMENT; + } + + /* 4. CRC-16/CCITT-FALSE over payload bytes (buf[16..len-3]). + * Payload length = len - 18 (16-byte header + 2-byte CRC trailer). + * Stored CRC is big-endian at buf[len-2..len-1]. */ + crc_computed = SIM_ADAPTER_Crc16(buf + 16U, (uint16)(len - 18U)); + crc_stored = (uint16)(((uint16)buf[len - 2U] << 8U) | (uint16)buf[len - 1U]); + + if (crc_computed != crc_stored) + { + CFE_EVS_SendEvent(SIM_ADAPTER_EID_CRC_MISMATCH, + CFE_EVS_EventType_ERROR, + "SIM_ADAPTER: CRC mismatch APID 0x%03X (got 0x%04X, expected 0x%04X)", + (unsigned int)apid, + (unsigned int)crc_stored, + (unsigned int)crc_computed); + SIM_ADAPTER_Data.CrcMismatches++; + return CFE_SB_BAD_ARGUMENT; + } + + /* 5. Route valid payload to SB. */ + SIM_ADAPTER_RouteToSB(apid, buf + 16U, len - 18U); + + return CFE_SUCCESS; +} + +/* --------------------------------------------------------------------------- + * SIM_ADAPTER_RouteToSB — Publish a validated fault SPP to the cFE Software Bus. + * + * Stub routing target is SIM_INJECT_HK_MID; a future phase maps each APID to + * its own MID once the full fault-injection table is wired (ICD-sim-fsw.md §5). + * --------------------------------------------------------------------------- */ +static void SIM_ADAPTER_RouteToSB(uint16 apid, const uint8 *payload, + uint32 payload_len) +{ + static CFE_SB_Buffer_t tlm_buf; + int32 status; + + (void)payload; + (void)payload_len; + + (void)CFE_MSG_SetMsgId(&tlm_buf.Msg, CFE_SB_ValueToMsgId(SIM_INJECT_HK_MID)); + (void)CFE_MSG_SetSize(&tlm_buf.Msg, (uint32)sizeof(CFE_SB_Buffer_t)); + + status = CFE_SB_TransmitMsg(&tlm_buf.Msg, true); + if (status != CFE_SUCCESS) + { + CFE_EVS_SendEvent(SIM_ADAPTER_EID_SB_TRANSMIT_ERR, + CFE_EVS_EventType_ERROR, + "SIM_ADAPTER: SB transmit failed 0x%08X APID 0x%03X", + (unsigned int)status, + (unsigned int)apid); + return; + } + + SIM_ADAPTER_Data.PacketsRouted++; + CFE_EVS_SendEvent(SIM_ADAPTER_EID_FAULT_APPLIED_INF_EID, + CFE_EVS_EventType_INFORMATION, + "SIM_ADAPTER: fault SPP routed APID 0x%03X total %u", + (unsigned int)apid, + (unsigned int)SIM_ADAPTER_Data.PacketsRouted); +} + +#endif /* !CFS_FLIGHT_BUILD */ diff --git a/apps/sim_adapter/fsw/src/sim_adapter.h b/apps/sim_adapter/fsw/src/sim_adapter.h new file mode 100644 index 0000000..c7dda00 --- /dev/null +++ b/apps/sim_adapter/fsw/src/sim_adapter.h @@ -0,0 +1,56 @@ +#ifndef SIM_ADAPTER_H +#define SIM_ADAPTER_H + +/* + * sim_adapter.h — SITL-only cFS application: UDP fault-SPP → Software Bus adapter. + * + * Receives CCSDS Space Packets from the fault_injector via UDP port 5700, + * validates APID (0x500–0x57F) and CRC-16/CCITT-FALSE, then routes valid + * packets onto the cFE Software Bus as SIM_INJECT_HK_MID messages. + * + * Entire implementation is excluded from flight builds by the + * CFS_FLIGHT_BUILD compile-time guard (ICD-sim-fsw.md §5.2). + */ + +#ifndef CFS_FLIGHT_BUILD + +#include "cfe.h" +#include "sim_adapter_events.h" +#include "sim_adapter_version.h" +#include "mids.h" + +/* ── Constants ───────────────────────────────────────────────────────────── */ +#define SIM_ADAPTER_PIPE_DEPTH 10U +#define SIM_ADAPTER_UDP_PORT 5700U +#define SIM_ADAPTER_FRAME_BUF_SIZE 1024U /* max UDP datagram; no VLA (MISRA Rule 18.8) */ +#define SIM_ADAPTER_APID_MIN 0x500U +#define SIM_ADAPTER_APID_MAX 0x57FU +#define SIM_ADAPTER_CRC16_POLY 0x1021U /* CRC-16/CCITT-FALSE polynomial */ +#define SIM_ADAPTER_CRC16_INIT 0xFFFFU /* CRC-16/CCITT-FALSE initial value */ + +/* ── Application state ───────────────────────────────────────────────────── */ +typedef struct { + CFE_EVS_BinFilter_t EventFilters[SIM_ADAPTER_EVT_COUNT]; + CFE_SB_PipeId_t CmdPipe; + osal_id_t UdpSockId; + OS_SockAddr_t UdpBindAddr; + uint32 PacketsRouted; + uint32 CrcMismatches; + uint32 ApidRejects; + uint32 RunStatus; + uint32 UptimeSeconds; +} SIM_ADAPTER_AppData_t; + +/* ── Entry point ─────────────────────────────────────────────────────────── */ +void SIM_ADAPTER_AppMain(void); + +/* ── Unit-test surface (non-static under UNIT_TEST) ─────────────────────── */ +#ifdef UNIT_TEST +extern SIM_ADAPTER_AppData_t SIM_ADAPTER_Data; +int32 SIM_ADAPTER_ProcessUdp(const uint8 *buf, uint32 len); +uint16 SIM_ADAPTER_Crc16(const uint8 *data, uint16 len); +#endif /* UNIT_TEST */ + +#endif /* !CFS_FLIGHT_BUILD */ + +#endif /* SIM_ADAPTER_H */ diff --git a/apps/sim_adapter/fsw/src/sim_adapter_events.h b/apps/sim_adapter/fsw/src/sim_adapter_events.h new file mode 100644 index 0000000..1f98811 --- /dev/null +++ b/apps/sim_adapter/fsw/src/sim_adapter_events.h @@ -0,0 +1,19 @@ +#ifndef SIM_ADAPTER_EVENTS_H +#define SIM_ADAPTER_EVENTS_H + +/* Event ID constants for sim_adapter. + * Source of truth: ICD-sim-fsw.md §6. */ + +#define SIM_ADAPTER_STARTUP_INF_EID 1U /* Initialization complete */ +#define SIM_ADAPTER_EID_PACKET_TOO_SHORT 2U /* UDP datagram < 18 bytes */ +#define SIM_ADAPTER_EID_BAD_HEADER 3U /* Primary header version error */ +#define SIM_ADAPTER_EID_APID_OUT_OF_RANGE 4U /* APID outside 0x500–0x57F */ +#define SIM_ADAPTER_EID_CRC_MISMATCH 5U /* CRC-16/CCITT-FALSE mismatch */ +#define SIM_ADAPTER_EID_FAULT_APPLIED_INF_EID 6U /* Fault SPP routed to SB */ +#define SIM_ADAPTER_EID_UNKNOWN_APID 7U /* APID in range but unregistered */ +#define SIM_ADAPTER_EID_INIT_SOCKET_ERR 8U /* OSAL UDP socket init failure */ +#define SIM_ADAPTER_EID_SB_TRANSMIT_ERR 9U /* CFE_SB_TransmitMsg failure */ + +#define SIM_ADAPTER_EVT_COUNT 9U + +#endif /* SIM_ADAPTER_EVENTS_H */ diff --git a/apps/sim_adapter/fsw/src/sim_adapter_version.h b/apps/sim_adapter/fsw/src/sim_adapter_version.h new file mode 100644 index 0000000..6adad89 --- /dev/null +++ b/apps/sim_adapter/fsw/src/sim_adapter_version.h @@ -0,0 +1,8 @@ +#ifndef SIM_ADAPTER_VERSION_H +#define SIM_ADAPTER_VERSION_H + +#define SIM_ADAPTER_MAJOR_VERSION 1 +#define SIM_ADAPTER_MINOR_VERSION 0 +#define SIM_ADAPTER_REVISION 0 + +#endif /* SIM_ADAPTER_VERSION_H */ diff --git a/apps/sim_adapter/fsw/unit-test/sim_adapter_test.c b/apps/sim_adapter/fsw/unit-test/sim_adapter_test.c new file mode 100644 index 0000000..b5be08c --- /dev/null +++ b/apps/sim_adapter/fsw/unit-test/sim_adapter_test.c @@ -0,0 +1,312 @@ +/* + * sim_adapter_test.c — CMocka unit tests for sim_adapter. + * + * CFE and OSAL API calls are intercepted via UNIT_TEST guards; no real cFE + * library is linked. SIM_ADAPTER_ProcessUdp and SIM_ADAPTER_Crc16 are exposed + * as non-static under UNIT_TEST so tests can call them directly. + * + * Coverage target: 100% branch coverage of sim_adapter.c. + */ + +#include +#include +#include +#include +#include + +#include "sim_adapter.h" + +/* --------------------------------------------------------------------------- + * CFE / OSAL stub implementations (UNIT_TEST only) + * --------------------------------------------------------------------------- */ +#ifdef UNIT_TEST + +int32 CFE_ES_RegisterApp(void) { return (int32)mock(); } + +int32 CFE_EVS_Register(const void *F, uint16 N, uint16 S) +{ + (void)F; (void)N; (void)S; + return (int32)mock(); +} + +int32 CFE_SB_CreatePipe(CFE_SB_PipeId_t *P, uint16 D, const char *N) +{ + (void)D; (void)N; + *P = (CFE_SB_PipeId_t)mock(); + return (int32)mock(); +} + +int32 CFE_SB_Subscribe(CFE_SB_MsgId_t MsgId, CFE_SB_PipeId_t PipeId) +{ + (void)MsgId; (void)PipeId; + return (int32)mock(); +} + +void CFE_EVS_SendEvent(uint16 EventID, uint16 EventType, const char *Spec, ...) +{ + (void)EventID; (void)EventType; (void)Spec; + function_called(); +} + +bool CFE_ES_RunLoop(uint32 *RunStatus) +{ + (void)RunStatus; + return (bool)mock(); +} + +int32 CFE_SB_ReceiveBuffer(CFE_SB_Buffer_t **BufPtr, CFE_SB_PipeId_t PipeId, int32 TimeOut) +{ + (void)PipeId; (void)TimeOut; + *BufPtr = (CFE_SB_Buffer_t *)mock(); + return (int32)mock(); +} + +int32 CFE_SB_TransmitMsg(CFE_MSG_Message_t *MsgPtr, bool Inc) +{ + (void)MsgPtr; (void)Inc; + function_called(); + return (int32)mock(); +} + +void CFE_ES_ExitApp(uint32 ExitStatus) { (void)ExitStatus; } + +int32 CFE_MSG_GetMsgId(const CFE_MSG_Message_t *M, CFE_SB_MsgId_t *Id) +{ + (void)M; + *Id = (CFE_SB_MsgId_t)mock(); + return CFE_SUCCESS; +} + +int32 CFE_MSG_GetFcnCode(const CFE_MSG_Message_t *M, CFE_MSG_FcnCode_t *C) +{ + (void)M; + *C = (CFE_MSG_FcnCode_t)mock(); + return CFE_SUCCESS; +} + +int32 CFE_MSG_SetMsgId(CFE_MSG_Message_t *MsgPtr, CFE_SB_MsgId_t MsgId) +{ + (void)MsgPtr; (void)MsgId; + return CFE_SUCCESS; +} + +int32 CFE_MSG_SetSize(CFE_MSG_Message_t *MsgPtr, uint32 TotalMsgSize) +{ + (void)MsgPtr; (void)TotalMsgSize; + return CFE_SUCCESS; +} + +CFE_SB_MsgId_Atom_t CFE_SB_MsgIdToValue(CFE_SB_MsgId_t MsgId) +{ + return (CFE_SB_MsgId_Atom_t)MsgId; +} + +CFE_SB_MsgId_t CFE_SB_ValueToMsgId(CFE_SB_MsgId_Atom_t MsgIdValue) +{ + return (CFE_SB_MsgId_t)MsgIdValue; +} + +int32 OS_SocketOpen(osal_id_t *sock_id, OS_SocketDomain_t Domain, OS_SocketType_t Type) +{ + (void)Domain; (void)Type; + *sock_id = (osal_id_t)mock(); + return (int32)mock(); +} + +int32 OS_SocketAddrInit(OS_SockAddr_t *Addr, OS_SocketDomain_t Domain) +{ + (void)Addr; (void)Domain; + return (int32)mock(); +} + +int32 OS_SocketAddrSetPort(OS_SockAddr_t *Addr, uint16 PortNum) +{ + (void)Addr; (void)PortNum; + return (int32)mock(); +} + +int32 OS_SocketBind(osal_id_t sock_id, const OS_SockAddr_t *Addr) +{ + (void)sock_id; (void)Addr; + return (int32)mock(); +} + +int32 OS_SocketRecvFrom(osal_id_t sock_id, void *buf, uint32 buflen, + OS_SockAddr_t *RemoteAddr, int32 timeout_ms) +{ + (void)sock_id; (void)buf; (void)buflen; (void)RemoteAddr; (void)timeout_ms; + return (int32)mock(); +} + +#endif /* UNIT_TEST */ + +/* --------------------------------------------------------------------------- + * Helpers + * --------------------------------------------------------------------------- */ + +/* CRC-16/CCITT-FALSE canonical check vector (ASCII "123456789"). */ +static const uint8 CRC_VECTOR[] = { + 0x31U, 0x32U, 0x33U, 0x34U, 0x35U, 0x36U, 0x37U, 0x38U, 0x39U +}; +#define CRC_VECTOR_LEN ((uint16)9U) +#define CRC_VECTOR_EXPECTED ((uint16)0x29B1U) + +/* Minimal valid datagram: 16-byte header (APID=0x540) + 0-byte payload + 2-byte CRC. + * APID 0x540: buf[0]=0x05, buf[1]=0x40. + * CRC of 0 bytes = init = 0xFFFF → buf[16]=0xFF, buf[17]=0xFF. */ +static const uint8 VALID_PKT[] = { + 0x05U, 0x40U, /* APID = 0x540 */ + 0x00U, 0x00U, 0x00U, 0x00U, /* primary header bytes 2–5 */ + 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, /* secondary header bytes 0–5 */ + 0x00U, 0x00U, 0x00U, 0x00U, /* secondary header bytes 6–9 */ + 0xFFU, 0xFFU /* CRC of 0 payload bytes = 0xFFFF */ +}; +#define VALID_PKT_LEN ((uint32)18U) + +/* Same as VALID_PKT but with incorrect CRC bytes. */ +static const uint8 BAD_CRC_PKT[] = { + 0x05U, 0x40U, + 0x00U, 0x00U, 0x00U, 0x00U, + 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, + 0x00U, 0x00U, 0x00U, 0x00U, + 0x00U, 0x00U /* wrong CRC — correct would be 0xFFFF */ +}; +#define BAD_CRC_PKT_LEN ((uint32)18U) + +/* 18-byte datagram with APID 0x600 (out of range: 0x06 | 0x00). */ +static const uint8 OOR_APID_PKT[] = { + 0x06U, 0x00U, /* APID = 0x600 — outside [0x500, 0x57F] */ + 0x00U, 0x00U, 0x00U, 0x00U, + 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, + 0x00U, 0x00U, 0x00U, 0x00U, + 0xFFU, 0xFFU +}; +#define OOR_APID_PKT_LEN ((uint32)18U) + +/* 10-byte datagram — too short to pass the length gate. */ +static const uint8 SHORT_PKT[] = { + 0x05U, 0x40U, 0x00U, 0x00U, 0x00U, + 0x00U, 0x00U, 0x00U, 0x00U, 0x00U +}; +#define SHORT_PKT_LEN ((uint32)10U) + +/* Reset SIM_ADAPTER_Data before tests that inspect counters. */ +static int reset_data(void **state) +{ + (void)state; + memset(&SIM_ADAPTER_Data, 0, sizeof(SIM_ADAPTER_Data)); + return 0; +} + +/* --------------------------------------------------------------------------- + * Test 1: CRC-16/CCITT-FALSE known vector + * + * Crc16("123456789", 9) must equal 0x29B1. + * This exercises the entire CRC loop and verifies polynomial / init constants. + * --------------------------------------------------------------------------- */ +static void test_crc_known_vector(void **state) +{ + (void)state; + uint16 result = SIM_ADAPTER_Crc16(CRC_VECTOR, CRC_VECTOR_LEN); + assert_int_equal((int)result, (int)CRC_VECTOR_EXPECTED); +} + +/* --------------------------------------------------------------------------- + * Test 2: Short datagram rejected before APID extraction + * + * Given: 10-byte datagram (< 18) + * When: SIM_ADAPTER_ProcessUdp called + * Then: EID_PACKET_TOO_SHORT event; ApidRejects incremented; no SB transmit + * --------------------------------------------------------------------------- */ +static void test_short_packet_rejected(void **state) +{ + (void)state; + expect_function_call(CFE_EVS_SendEvent); /* EID_PACKET_TOO_SHORT */ + (void)SIM_ADAPTER_ProcessUdp(SHORT_PKT, SHORT_PKT_LEN); + assert_int_equal((int)SIM_ADAPTER_Data.ApidRejects, 1); +} + +/* --------------------------------------------------------------------------- + * Test 3: APID outside sideband block rejected + * + * Given: 18-byte datagram, APID=0x600 (outside 0x500–0x57F) + * When: SIM_ADAPTER_ProcessUdp called + * Then: EID_APID_OUT_OF_RANGE event; ApidRejects incremented; no SB transmit + * --------------------------------------------------------------------------- */ +static void test_apid_out_of_range_rejected(void **state) +{ + (void)state; + expect_function_call(CFE_EVS_SendEvent); /* EID_APID_OUT_OF_RANGE */ + (void)SIM_ADAPTER_ProcessUdp(OOR_APID_PKT, OOR_APID_PKT_LEN); + assert_int_equal((int)SIM_ADAPTER_Data.ApidRejects, 1); +} + +/* --------------------------------------------------------------------------- + * Test 4: CRC mismatch causes packet to be dropped + * + * Given: 18-byte datagram, APID=0x540, stored CRC=0x0000 (wrong) + * When: SIM_ADAPTER_ProcessUdp called + * Then: EID_CRC_MISMATCH event; CrcMismatches incremented; no SB transmit + * --------------------------------------------------------------------------- */ +static void test_crc_mismatch_rejected(void **state) +{ + (void)state; + expect_function_call(CFE_EVS_SendEvent); /* EID_CRC_MISMATCH */ + (void)SIM_ADAPTER_ProcessUdp(BAD_CRC_PKT, BAD_CRC_PKT_LEN); + assert_int_equal((int)SIM_ADAPTER_Data.CrcMismatches, 1); +} + +/* --------------------------------------------------------------------------- + * Test 5: Valid packet is routed to the Software Bus + * + * Given: 18-byte datagram, APID=0x540, CRC=0xFFFF (correct for 0 payload bytes) + * When: SIM_ADAPTER_ProcessUdp called + * Then: CFE_SB_TransmitMsg called once; PacketsRouted == 1 + * --------------------------------------------------------------------------- */ +static void test_valid_packet_routed(void **state) +{ + (void)state; + expect_function_call(CFE_SB_TransmitMsg); + will_return(CFE_SB_TransmitMsg, CFE_SUCCESS); + expect_function_call(CFE_EVS_SendEvent); /* EID_FAULT_APPLIED_INF_EID */ + (void)SIM_ADAPTER_ProcessUdp(VALID_PKT, VALID_PKT_LEN); + assert_int_equal((int)SIM_ADAPTER_Data.PacketsRouted, 1); +} + +/* --------------------------------------------------------------------------- + * Test 6: OS_SocketOpen failure causes Init to return an error + * + * Given: OS_SocketOpen returns a negative error code + * When: SIM_ADAPTER_AppMain called + * Then: EID_INIT_SOCKET_ERR event; RunLoop entered with APP_ERROR → exits immediately + * --------------------------------------------------------------------------- */ +static void test_init_socket_failure(void **state) +{ + (void)state; + will_return(CFE_ES_RegisterApp, CFE_SUCCESS); + will_return(CFE_EVS_Register, CFE_SUCCESS); + will_return(CFE_SB_CreatePipe, 1); + will_return(CFE_SB_CreatePipe, CFE_SUCCESS); + will_return(CFE_SB_Subscribe, CFE_SUCCESS); + will_return(OS_SocketOpen, 0); /* socket id (unused on failure) */ + will_return(OS_SocketOpen, (int32)-1); /* OS_ERR_SOCKET_CLOSED */ + expect_function_call(CFE_EVS_SendEvent); /* EID_INIT_SOCKET_ERR */ + will_return(CFE_ES_RunLoop, false); + SIM_ADAPTER_AppMain(); +} + +/* --------------------------------------------------------------------------- + * Test runner + * --------------------------------------------------------------------------- */ +int main(void) +{ + const struct CMUnitTest tests[] = { + cmocka_unit_test(test_crc_known_vector), + cmocka_unit_test_setup(test_short_packet_rejected, reset_data), + cmocka_unit_test_setup(test_apid_out_of_range_rejected, reset_data), + cmocka_unit_test_setup(test_crc_mismatch_rejected, reset_data), + cmocka_unit_test_setup(test_valid_packet_routed, reset_data), + cmocka_unit_test(test_init_socket_failure), + }; + return cmocka_run_group_tests(tests, NULL, NULL); +} diff --git a/compose.yaml b/compose.yaml index 9c2d454..532e9ff 100644 --- a/compose.yaml +++ b/compose.yaml @@ -44,11 +44,18 @@ services: retries: 10 start_period: 10s - # ── Gazebo Harmonic (headless) ──────────────────────────────────────────────── - # Runs gz sim server with mars_surrogate.sdf (Mars gravity, flat terrain, - # rover/UAV/cryobot models). Plugin .so files are not compiled in this - # image so Gazebo emits "plugin not found" warnings and continues without - # rover dynamics — full plugin support requires Phase A (HOW_TO_RUN.md §10). + # ── Gazebo Harmonic ─────────────────────────────────────────────────────────── + # Runs gz sim with mars_surrogate.sdf (Mars gravity, flat terrain, + # rover/UAV/cryobot models). Plugin .so files are compiled into the image + # by the multi-stage gazebo.Dockerfile (Phase A). + # GZ_IP and GZ_PARTITION must match the ros2 service for gz-transport peer + # discovery to work across Docker bridge network containers (Phase B). + # + # GUI: the command detects $$DISPLAY at runtime. + # With display (make run on a desktop): opens the full Gazebo 3D window. + # Without display (CI / headless server): runs server-only (-s flag). + # LIBGL_ALWAYS_SOFTWARE forces Mesa llvmpipe — no GPU passthrough required. + # The /tmp/.X11-unix bind-mount forwards the host X11 socket into the container. gazebo: image: sakura/gazebo:local container_name: sakura_gazebo @@ -57,7 +64,21 @@ services: dockerfile: .ci/gazebo.Dockerfile volumes: - ./simulation:/app/simulation:ro - command: gz sim -r -s /app/simulation/worlds/mars_surrogate.sdf + - /tmp/.X11-unix:/tmp/.X11-unix:rw + command: > + bash -c " + if [ -n \"$$DISPLAY\" ]; then + gz sim -r /app/simulation/worlds/mars_surrogate.sdf; + else + gz sim -r -s /app/simulation/worlds/mars_surrogate.sdf; + fi + " + environment: + - DISPLAY + - GZ_IP=gazebo + - GZ_PARTITION=sakura + - LIBGL_ALWAYS_SOFTWARE=1 + - MESA_GL_VERSION_OVERRIDE=3.3 networks: - sakura-net depends_on: @@ -71,9 +92,11 @@ services: start_period: 30s # ── Space ROS 2 ─────────────────────────────────────────────────────────────── - # Builds the rover workspace with colcon, then launches teleop_node via - # rover.launch.py (lifecycle node, starts in unconfigured state). - # sim.launch.py (Gazebo bridge) requires ros_gz_bridge — see Phase B. + # Builds the rover workspace with colcon, then launches via sim.launch.py + # which starts ros_gz_bridge (bridging /cmd_vel and odometry) and teleop_node. + # use_external_gz:=true skips launching a second gz sim process — the gazebo + # service above already owns the physics server. + # Set LAUNCH_MODE=rover to fall back to rover.launch.py (no bridge, no Gazebo). ros2: image: sakura/ros2:local container_name: sakura_ros2 @@ -88,15 +111,18 @@ services: cd /workspace && colcon build --base-paths ros2_ws/src --symlink-install && source install/setup.bash && - ros2 launch rover_bringup rover.launch.py + ros2 launch rover_bringup ${LAUNCH_MODE:-sim}.launch.py use_external_gz:=true " environment: - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp + - GZ_IP=gazebo + - GZ_PARTITION=sakura + - LAUNCH_MODE=sim networks: - sakura-net depends_on: gazebo: - condition: service_started + condition: service_healthy healthcheck: test: ["CMD-SHELL", "pgrep -f teleop_node || exit 1"] interval: 5s diff --git a/cpu1/startup_scripts/cfe_es_startup.scr b/cpu1/startup_scripts/cfe_es_startup.scr index 0d90fef..8bd9210 100644 --- a/cpu1/startup_scripts/cfe_es_startup.scr +++ b/cpu1/startup_scripts/cfe_es_startup.scr @@ -20,6 +20,7 @@ CFE_APP, /cf/liborbiter_comm.so, ORBITER_COMM_AppMain, ORBITER_COMM, CFE_APP, /cf/liborbiter_power.so, ORBITER_POWER_AppMain, ORBITER_POWER, 85, 16384, 0x0, 0x0; CFE_APP, /cf/liborbiter_payload.so, ORBITER_PAYLOAD_AppMain, ORBITER_PAYLOAD, 86, 16384, 0x0, 0x0; CFE_APP, /cf/libmcu_eps_gw.so, MCU_EPS_GW_AppMain, MCU_EPS_GW, 90, 16384, 0x0, 0x0; +CFE_APP, /cf/libsim_adapter.so, SIM_ADAPTER_AppMain, SIM_ADAPTER, 95, 16384, 0x0, 0x0; CFE_APP, /cf/libmcu_rwa_gw.so, MCU_RWA_GW_AppMain, MCU_RWA_GW, 90, 16384, 0x0, 0x0; CFE_APP, /cf/libmcu_payload_gw.so, MCU_PAYLOAD_GW_AppMain, MCU_PAYLOAD_GW, 90, 16384, 0x0, 0x0; CFE_APP, /cf/liborbiter_cdh.so, ORBITER_CDH_AppMain, ORBITER_CDH, 100, 32768, 0x0, 0x0; diff --git a/docs/interfaces/apid-registry.md b/docs/interfaces/apid-registry.md index e552027..dc67d93 100644 --- a/docs/interfaces/apid-registry.md +++ b/docs/interfaces/apid-registry.md @@ -106,6 +106,17 @@ Each FreeRTOS MCU class gets a 16-APID block. Per-instance multiplicity is handl | `0x2A0`–`0x2AF` | `mcu_eps` | UART | | `0x2B0`–`0x2FF` | *reserved* | — | +## Sim Injection Sub-Allocation (0x500–0x57F) + +SITL-only; never placed on any RF link. `sim_adapter` receives these on UDP port 5700 and routes valid packets to the cFE Software Bus. `CFS_FLIGHT_BUILD` guard ensures zero symbols in flight image. + +| APID | Macro | Purpose | +|---|---|---| +| `0x501` | `SIM_ADAPTER_HK_MID` | `sim_adapter` app housekeeping telemetry | +| `0x502`–`0x53F` | *reserved* | Unallocated sim sideband | +| `0x540`–`0x543` | *reserved* | Fault injection SPP APIDs (raw; no cFS MID; see `SIM_FAULT_APID_*` in `_defs/mids.h`) | +| `0x544`–`0x57F` | *reserved* | Unallocated | + ## AOS Virtual Channel (VC) Allocation AOS Transfer Frames multiplex multiple data streams on a single link. SAKURA-II uses three VCs on the orbiter-to-ground downlink (CCSDS 732.0-B): diff --git a/ros2_ws/src/rover_bringup/launch/sim.launch.py b/ros2_ws/src/rover_bringup/launch/sim.launch.py index d9dab9d..c3b8113 100644 --- a/ros2_ws/src/rover_bringup/launch/sim.launch.py +++ b/ros2_ws/src/rover_bringup/launch/sim.launch.py @@ -25,6 +25,7 @@ ExecuteProcess, TimerAction, ) +from launch.conditions import UnlessCondition from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import LifecycleNode, Node from launch_ros.substitutions import FindPackageShare @@ -62,6 +63,15 @@ def generate_launch_description() -> LaunchDescription: description="Run Gazebo without GUI (true=headless, false=with GUI)", ) + # When running inside Docker Compose the gazebo service already owns the + # gz sim process. Set use_external_gz:=true to skip starting a second + # instance and only launch the bridge and teleop_node. + use_external_gz_arg = DeclareLaunchArgument( + "use_external_gz", + default_value="false", + description="Skip launching the Gazebo server (use when Gazebo runs externally)", + ) + # ── Process 1: Gazebo Harmonic server ──────────────────────────────────── # gz sim -r -s loads the world in headless (server-only) mode. # The -r flag starts physics immediately (no manual 'play' required). @@ -75,6 +85,7 @@ def generate_launch_description() -> LaunchDescription: ], output="screen", name="gz_sim_server", + condition=UnlessCondition(LaunchConfiguration("use_external_gz")), ) # ── Process 2: ros_gz_bridge ───────────────────────────────────────────── @@ -134,6 +145,7 @@ def generate_launch_description() -> LaunchDescription: params_file_arg, sdf_path_arg, headless_arg, + use_external_gz_arg, gz_server, gz_bridge, teleop_node, diff --git a/simulation/gazebo_cryobot_plugin/CMakeLists.txt b/simulation/gazebo_cryobot_plugin/CMakeLists.txt index 297e2aa..e8c3865 100644 --- a/simulation/gazebo_cryobot_plugin/CMakeLists.txt +++ b/simulation/gazebo_cryobot_plugin/CMakeLists.txt @@ -15,25 +15,24 @@ if(BUILD_TESTING) endif() # ── Gazebo shared library ───────────────────────────────────────────────────── -find_package(gazebo QUIET) +find_package(gz-sim8 QUIET) +find_package(gz-transport13 QUIET) +find_package(gz-plugin2 QUIET) -if(NOT gazebo_FOUND) - message(WARNING "Gazebo not found — cryobot physics plugin will not be built.") +if(NOT gz-sim8_FOUND) + message(WARNING "gz-sim8 not found — cryobot physics plugin will not be built.") return() endif() -include_directories( - include - ${GAZEBO_INCLUDE_DIRS} -) -link_directories(${GAZEBO_LIBRARY_DIRS}) -list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}") - add_library(cryobot_physics_plugin SHARED src/cryobot_physics_plugin.cpp ) target_include_directories(cryobot_physics_plugin PRIVATE include) target_compile_options(cryobot_physics_plugin PRIVATE -Wall -Wextra -Werror) -target_link_libraries(cryobot_physics_plugin ${GAZEBO_LIBRARIES}) +target_link_libraries(cryobot_physics_plugin + gz-sim8::gz-sim8 + gz-transport13::gz-transport13 + gz-plugin2::gz-plugin2 +) install(TARGETS cryobot_physics_plugin DESTINATION lib) diff --git a/simulation/gazebo_cryobot_plugin/include/cryobot_physics_plugin.h b/simulation/gazebo_cryobot_plugin/include/cryobot_physics_plugin.h index 77b4746..4b90fb2 100644 --- a/simulation/gazebo_cryobot_plugin/include/cryobot_physics_plugin.h +++ b/simulation/gazebo_cryobot_plugin/include/cryobot_physics_plugin.h @@ -1,50 +1,50 @@ #pragma once -#include -#include -#include -#include +#include +#include +#include +#include +#include #include - -namespace gazebo -{ +#include /** - * CrybotPhysicsPlugin — Gazebo ModelPlugin for a tethered subsurface cryobot. + * CrybotPhysicsPlugin — Gazebo Harmonic system plugin for a tethered cryobot. * * Attach to a model SDF element: * * - * Models tether tension and ice-penetration force. Subscribes to - * "~//cmd_vel" for downward velocity commands. - * OnUpdate() applies net axial force each simulation step. + * Models tether spring restoring force and descent thrust. Subscribes to + * "/model//cmd_vel" for downward velocity commands (linear.z). + * PreUpdate() applies net axial force to the base link each physics step. + * + * Phase 38: tether extension derived from world-Z pose; PD controller lands + * in Phase 42. */ -class CrybotPhysicsPlugin : public ModelPlugin +class CrybotPhysicsPlugin : + public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPreUpdate { public: - CrybotPhysicsPlugin(); - ~CrybotPhysicsPlugin() override; + void Configure(const gz::sim::Entity &entity, + const std::shared_ptr &sdf, + gz::sim::EntityComponentManager &ecm, + gz::sim::EventManager &eventMgr) override; - void Load(physics::ModelPtr model, sdf::ElementPtr sdf) override; - void Reset() override; + void PreUpdate(const gz::sim::UpdateInfo &info, + gz::sim::EntityComponentManager &ecm) override; private: - void OnUpdate(); - void OnCmdVel(ConstTwistPtr &msg); + void OnCmdVel(const gz::msgs::Twist &msg); - physics::ModelPtr model_; - event::ConnectionPtr update_connection_; + gz::sim::Entity entity_{gz::sim::kNullEntity}; + gz::sim::Entity linkEntity_{gz::sim::kNullEntity}; - transport::NodePtr node_; - transport::SubscriberPtr cmd_vel_sub_; + gz::transport::Node node_; - /* Tether tension magnitude [N]; updated each physics step based on depth. */ double tether_tension_n_{0.0}; double descent_rate_ms_{0.0}; std::mutex cmd_vel_mutex_; }; - -GZ_REGISTER_MODEL_PLUGIN(CrybotPhysicsPlugin) - -} // namespace gazebo diff --git a/simulation/gazebo_cryobot_plugin/src/cryobot_physics_plugin.cpp b/simulation/gazebo_cryobot_plugin/src/cryobot_physics_plugin.cpp index 9e08dbd..68d9a38 100644 --- a/simulation/gazebo_cryobot_plugin/src/cryobot_physics_plugin.cpp +++ b/simulation/gazebo_cryobot_plugin/src/cryobot_physics_plugin.cpp @@ -1,79 +1,56 @@ #include "cryobot_physics_plugin.h" #include "cryobot_physics_core.h" -#include - -#include - -namespace gazebo -{ - -CrybotPhysicsPlugin::CrybotPhysicsPlugin() -: model_(nullptr) -{ -} - -CrybotPhysicsPlugin::~CrybotPhysicsPlugin() -{ - /* update_connection_ RAII destructor disconnects the WorldUpdateBegin - * signal automatically. */ -} - -void CrybotPhysicsPlugin::Load(physics::ModelPtr model, sdf::ElementPtr /*sdf*/) +#include +#include +#include +#include +#include + +#include +#include + +void CrybotPhysicsPlugin::Configure( + const gz::sim::Entity &entity, + const std::shared_ptr &, + gz::sim::EntityComponentManager &ecm, + gz::sim::EventManager &) { - if (!model) - { - gzerr << "[CrybotPhysicsPlugin] Load called with null model pointer\n"; - return; + entity_ = entity; + gz::sim::Model model(entity); + + /* Use the first link as the base link for tether force application. */ + const std::vector links = model.Links(ecm); + if (!links.empty()) { + linkEntity_ = links.front(); + ecm.CreateComponent(linkEntity_, + gz::sim::components::ExternalWorldWrenchCmd()); + } else { + gzerr << "[CrybotPhysicsPlugin] No links found on model — forces will not be applied\n"; } - model_ = model; - - node_ = transport::NodePtr(new transport::Node()); - node_->Init(); - const std::string topic = std::string("~/") + model_->GetName() + "/cmd_vel"; - cmd_vel_sub_ = node_->Subscribe(topic, &CrybotPhysicsPlugin::OnCmdVel, this); - - update_connection_ = event::Events::ConnectWorldUpdateBegin( - std::bind(&CrybotPhysicsPlugin::OnUpdate, this)); + const std::string modelName = model.Name(ecm); + const std::string topic = "/model/" + modelName + "/cmd_vel"; + node_.Subscribe(topic, &CrybotPhysicsPlugin::OnCmdVel, this); - gzmsg << "[CrybotPhysicsPlugin] Loaded on model: " << model_->GetName() + gzmsg << "[CrybotPhysicsPlugin] Loaded on model: " << modelName << " — cmd_vel topic: " << topic << "\n"; } -void CrybotPhysicsPlugin::Reset() -{ - std::lock_guard lock(cmd_vel_mutex_); - tether_tension_n_ = 0.0; - descent_rate_ms_ = 0.0; -} - -void CrybotPhysicsPlugin::OnCmdVel(ConstTwistPtr &msg) +void CrybotPhysicsPlugin::PreUpdate( + const gz::sim::UpdateInfo &, + gz::sim::EntityComponentManager &ecm) { - std::lock_guard lock(cmd_vel_mutex_); - /* linear.z is the commanded descent rate (negative = down in NED). */ - descent_rate_ms_ = msg->linear().z(); -} - -void CrybotPhysicsPlugin::OnUpdate() -{ - if (!model_) - { - return; - } - - auto links = model_->GetLinks(); - if (links.empty()) - { + if (linkEntity_ == gz::sim::kNullEntity) return; - } - - auto base = links.front(); - /* Compute approximate depth from world origin as a proxy for tether - * extension. A full Phase 42+ implementation reads the tether joint - * state directly. */ - const double depth = -base->GetWorldPose().pos.z; + /* Compute approximate depth from world Z as proxy for tether extension. + * Phase 42+ reads the tether joint state directly. */ + const auto *poseComp = + ecm.Component(linkEntity_); + const double depth = (poseComp != nullptr) + ? -poseComp->Data().Pos().Z() + : 0.0; double descent = 0.0; { @@ -88,9 +65,26 @@ void CrybotPhysicsPlugin::OnUpdate() tether_tension_n_ = step.tether_tension_n; } - /* Apply net axial force: tether restoring (upward) + descent thrust. - * Phase 42+ replaces the proportional mapping with a PD controller. */ - base->AddRelativeForce(math::Vector3(0.0, 0.0, step.net_force_z)); + /* Apply net axial force: tether restoring (upward) + descent thrust. */ + auto *wrenchCmd = ecm.Component(linkEntity_); + if (wrenchCmd) { + gz::msgs::Wrench wrench; + wrench.mutable_force()->set_x(0.0); + wrench.mutable_force()->set_y(0.0); + wrench.mutable_force()->set_z(step.net_force_z); + wrench.mutable_torque()->set_x(0.0); + wrench.mutable_torque()->set_y(0.0); + wrench.mutable_torque()->set_z(0.0); + wrenchCmd->Data() = wrench; + } +} + +void CrybotPhysicsPlugin::OnCmdVel(const gz::msgs::Twist &msg) +{ + std::lock_guard lock(cmd_vel_mutex_); + /* linear.z is the commanded descent rate (negative = down in NED). */ + descent_rate_ms_ = msg.linear().z(); } -} // namespace gazebo +GZ_ADD_PLUGIN(CrybotPhysicsPlugin, gz::sim::System, + gz::sim::ISystemConfigure, gz::sim::ISystemPreUpdate) diff --git a/simulation/gazebo_rover_plugin/CMakeLists.txt b/simulation/gazebo_rover_plugin/CMakeLists.txt index 0401851..e97c799 100644 --- a/simulation/gazebo_rover_plugin/CMakeLists.txt +++ b/simulation/gazebo_rover_plugin/CMakeLists.txt @@ -4,25 +4,24 @@ project(gazebo_rover_plugin CXX) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) -find_package(gazebo QUIET) +find_package(gz-sim8 QUIET) +find_package(gz-transport13 QUIET) +find_package(gz-plugin2 QUIET) -if(NOT gazebo_FOUND) - message(WARNING "Gazebo not found — rover drive plugin will not be built. Install Gazebo Harmonic to enable.") +if(NOT gz-sim8_FOUND) + message(WARNING "gz-sim8 not found — rover drive plugin will not be built. Install Gazebo Harmonic to enable.") return() endif() -include_directories( - include - ${GAZEBO_INCLUDE_DIRS} -) -link_directories(${GAZEBO_LIBRARY_DIRS}) -list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}") - add_library(rover_drive_plugin SHARED src/rover_drive_plugin.cpp ) target_include_directories(rover_drive_plugin PRIVATE include) target_compile_options(rover_drive_plugin PRIVATE -Wall -Wextra -Werror) -target_link_libraries(rover_drive_plugin ${GAZEBO_LIBRARIES}) +target_link_libraries(rover_drive_plugin + gz-sim8::gz-sim8 + gz-transport13::gz-transport13 + gz-plugin2::gz-plugin2 +) install(TARGETS rover_drive_plugin DESTINATION lib) diff --git a/simulation/gazebo_rover_plugin/include/rover_drive_plugin.h b/simulation/gazebo_rover_plugin/include/rover_drive_plugin.h index 3365a92..acfe6e6 100644 --- a/simulation/gazebo_rover_plugin/include/rover_drive_plugin.h +++ b/simulation/gazebo_rover_plugin/include/rover_drive_plugin.h @@ -1,49 +1,49 @@ #pragma once -#include -#include -#include -#include +#include +#include +#include +#include +#include #include - -namespace gazebo -{ +#include /** - * RoverDrivePlugin — Gazebo ModelPlugin for a wheeled rover chassis. + * RoverDrivePlugin — Gazebo Harmonic system plugin for a wheeled rover chassis. * * Attach to a model SDF element: * * - * Subscribes to "~//cmd_vel" for differential-drive velocity - * commands (linear.x = forward m/s, angular.z = yaw rad/s). - * OnUpdate() is called every simulation step. It must not block. + * Subscribes to "/model//cmd_vel" (gz::msgs::Twist) for + * differential-drive velocity commands (linear.x = forward m/s, + * angular.z = yaw rad/s). PreUpdate() applies per-wheel angular velocity + * to left_wheel_joint and right_wheel_joint via JointVelocityCmd each step. */ -class RoverDrivePlugin : public ModelPlugin +class RoverDrivePlugin : + public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPreUpdate { public: - RoverDrivePlugin(); - ~RoverDrivePlugin() override; + void Configure(const gz::sim::Entity &entity, + const std::shared_ptr &sdf, + gz::sim::EntityComponentManager &ecm, + gz::sim::EventManager &eventMgr) override; - void Load(physics::ModelPtr model, sdf::ElementPtr sdf) override; - void Reset() override; + void PreUpdate(const gz::sim::UpdateInfo &info, + gz::sim::EntityComponentManager &ecm) override; private: - void OnUpdate(); - void OnCmdVel(ConstTwistPtr &msg); + void OnCmdVel(const gz::msgs::Twist &msg); - physics::ModelPtr model_; - event::ConnectionPtr update_connection_; + gz::sim::Entity entity_{gz::sim::kNullEntity}; + gz::sim::Entity leftJointEntity_{gz::sim::kNullEntity}; + gz::sim::Entity rightJointEntity_{gz::sim::kNullEntity}; - transport::NodePtr node_; - transport::SubscriberPtr cmd_vel_sub_; + gz::transport::Node node_; double lin_vel_{0.0}; double ang_vel_{0.0}; std::mutex cmd_vel_mutex_; }; - -GZ_REGISTER_MODEL_PLUGIN(RoverDrivePlugin) - -} // namespace gazebo diff --git a/simulation/gazebo_rover_plugin/src/rover_drive_plugin.cpp b/simulation/gazebo_rover_plugin/src/rover_drive_plugin.cpp index 62a15ee..ac27ce0 100644 --- a/simulation/gazebo_rover_plugin/src/rover_drive_plugin.cpp +++ b/simulation/gazebo_rover_plugin/src/rover_drive_plugin.cpp @@ -1,71 +1,46 @@ #include "rover_drive_plugin.h" -#include -#include +#include +#include +#include -#include +#include -namespace gazebo -{ - -/* Physical parameters of the rover chassis used for differential-drive kinematics. */ -static constexpr double WHEEL_RADIUS_M = 0.1; /* wheel radius [m] */ -static constexpr double HALF_TRACK_M = 0.2; /* half track width [m] */ - -RoverDrivePlugin::RoverDrivePlugin() -: model_(nullptr) -{ -} - -RoverDrivePlugin::~RoverDrivePlugin() -{ - /* update_connection_ RAII destructor disconnects the WorldUpdateBegin - * signal automatically — no explicit Disconnect() call needed. */ -} +/* Physical parameters of the rover chassis. */ +static constexpr double WHEEL_RADIUS_M = 0.1; +static constexpr double HALF_TRACK_M = 0.2; -void RoverDrivePlugin::Load(physics::ModelPtr model, sdf::ElementPtr /*sdf*/) +void RoverDrivePlugin::Configure( + const gz::sim::Entity &entity, + const std::shared_ptr &, + gz::sim::EntityComponentManager &ecm, + gz::sim::EventManager &) { - if (!model) - { - gzerr << "[RoverDrivePlugin] Load called with null model pointer\n"; - return; - } + entity_ = entity; + gz::sim::Model model(entity); - model_ = model; + leftJointEntity_ = model.JointByName(ecm, "left_wheel_joint"); + rightJointEntity_ = model.JointByName(ecm, "right_wheel_joint"); - node_ = transport::NodePtr(new transport::Node()); - node_->Init(); - const std::string topic = std::string("~/") + model_->GetName() + "/cmd_vel"; - cmd_vel_sub_ = node_->Subscribe(topic, &RoverDrivePlugin::OnCmdVel, this); + if (leftJointEntity_ != gz::sim::kNullEntity) + ecm.CreateComponent(leftJointEntity_, + gz::sim::components::JointVelocityCmd({0.0})); + if (rightJointEntity_ != gz::sim::kNullEntity) + ecm.CreateComponent(rightJointEntity_, + gz::sim::components::JointVelocityCmd({0.0})); - update_connection_ = event::Events::ConnectWorldUpdateBegin( - std::bind(&RoverDrivePlugin::OnUpdate, this)); + const std::string modelName = model.Name(ecm); + const std::string topic = "/model/" + modelName + "/cmd_vel"; + node_.Subscribe(topic, &RoverDrivePlugin::OnCmdVel, this); - gzmsg << "[RoverDrivePlugin] Loaded on model: " << model_->GetName() + gzmsg << "[RoverDrivePlugin] Loaded on model: " << modelName << " — cmd_vel topic: " << topic << "\n"; } -void RoverDrivePlugin::Reset() -{ - std::lock_guard lock(cmd_vel_mutex_); - lin_vel_ = 0.0; - ang_vel_ = 0.0; -} - -void RoverDrivePlugin::OnCmdVel(ConstTwistPtr &msg) +void RoverDrivePlugin::PreUpdate( + const gz::sim::UpdateInfo &, + gz::sim::EntityComponentManager &ecm) { - std::lock_guard lock(cmd_vel_mutex_); - lin_vel_ = msg->linear().x(); - ang_vel_ = msg->angular().z(); -} - -void RoverDrivePlugin::OnUpdate() -{ - if (!model_) - { - return; - } - double lin = 0.0; double ang = 0.0; { @@ -78,17 +53,22 @@ void RoverDrivePlugin::OnUpdate() const double left_rad_s = (lin - ang * HALF_TRACK_M) / WHEEL_RADIUS_M; const double right_rad_s = (lin + ang * HALF_TRACK_M) / WHEEL_RADIUS_M; - auto left_joint = model_->GetJoint("left_wheel_joint"); - auto right_joint = model_->GetJoint("right_wheel_joint"); - - if (left_joint) - { - left_joint->SetVelocity(0U, left_rad_s); + if (leftJointEntity_ != gz::sim::kNullEntity) { + auto *cmd = ecm.Component(leftJointEntity_); + if (cmd) cmd->Data() = {left_rad_s}; } - if (right_joint) - { - right_joint->SetVelocity(0U, right_rad_s); + if (rightJointEntity_ != gz::sim::kNullEntity) { + auto *cmd = ecm.Component(rightJointEntity_); + if (cmd) cmd->Data() = {right_rad_s}; } } -} // namespace gazebo +void RoverDrivePlugin::OnCmdVel(const gz::msgs::Twist &msg) +{ + std::lock_guard lock(cmd_vel_mutex_); + lin_vel_ = msg.linear().x(); + ang_vel_ = msg.angular().z(); +} + +GZ_ADD_PLUGIN(RoverDrivePlugin, gz::sim::System, + gz::sim::ISystemConfigure, gz::sim::ISystemPreUpdate) diff --git a/simulation/gazebo_uav_plugin/CMakeLists.txt b/simulation/gazebo_uav_plugin/CMakeLists.txt index b7367e3..5d42e20 100644 --- a/simulation/gazebo_uav_plugin/CMakeLists.txt +++ b/simulation/gazebo_uav_plugin/CMakeLists.txt @@ -15,25 +15,24 @@ if(BUILD_TESTING) endif() # ── Gazebo shared library ───────────────────────────────────────────────────── -find_package(gazebo QUIET) +find_package(gz-sim8 QUIET) +find_package(gz-transport13 QUIET) +find_package(gz-plugin2 QUIET) -if(NOT gazebo_FOUND) - message(WARNING "Gazebo not found — UAV flight plugin will not be built.") +if(NOT gz-sim8_FOUND) + message(WARNING "gz-sim8 not found — UAV flight plugin will not be built.") return() endif() -include_directories( - include - ${GAZEBO_INCLUDE_DIRS} -) -link_directories(${GAZEBO_LIBRARY_DIRS}) -list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}") - add_library(uav_flight_plugin SHARED src/uav_flight_plugin.cpp ) target_include_directories(uav_flight_plugin PRIVATE include) target_compile_options(uav_flight_plugin PRIVATE -Wall -Wextra -Werror) -target_link_libraries(uav_flight_plugin ${GAZEBO_LIBRARIES}) +target_link_libraries(uav_flight_plugin + gz-sim8::gz-sim8 + gz-transport13::gz-transport13 + gz-plugin2::gz-plugin2 +) install(TARGETS uav_flight_plugin DESTINATION lib) diff --git a/simulation/gazebo_uav_plugin/include/uav_flight_plugin.h b/simulation/gazebo_uav_plugin/include/uav_flight_plugin.h index aa5ec27..897b8c3 100644 --- a/simulation/gazebo_uav_plugin/include/uav_flight_plugin.h +++ b/simulation/gazebo_uav_plugin/include/uav_flight_plugin.h @@ -1,48 +1,50 @@ #pragma once -#include -#include -#include -#include +#include +#include +#include +#include +#include #include - -namespace gazebo -{ +#include /** - * UavFlightPlugin — Gazebo ModelPlugin for a UAV (fixed or rotary wing). + * UavFlightPlugin — Gazebo Harmonic system plugin for a UAV (rotary wing). * * Attach to a model SDF element: * * - * Subscribes to "~//cmd_vel" for attitude + thrust commands. - * OnUpdate() applies torques to rotor joints each simulation step. + * Subscribes to "/model//cmd_vel" for thrust + yaw commands + * (linear.z = collective thrust N, angular.z = yaw rate rad/s). + * PreUpdate() applies net force/torque to the base link via + * ExternalWorldWrenchCmd each physics step. + * + * Phase 38: direct mapping; per-rotor torque model lands in Phase 42. */ -class UavFlightPlugin : public ModelPlugin +class UavFlightPlugin : + public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPreUpdate { public: - UavFlightPlugin(); - ~UavFlightPlugin() override; + void Configure(const gz::sim::Entity &entity, + const std::shared_ptr &sdf, + gz::sim::EntityComponentManager &ecm, + gz::sim::EventManager &eventMgr) override; - void Load(physics::ModelPtr model, sdf::ElementPtr sdf) override; - void Reset() override; + void PreUpdate(const gz::sim::UpdateInfo &info, + gz::sim::EntityComponentManager &ecm) override; private: - void OnUpdate(); - void OnCmdVel(ConstTwistPtr &msg); + void OnCmdVel(const gz::msgs::Twist &msg); - physics::ModelPtr model_; - event::ConnectionPtr update_connection_; + gz::sim::Entity entity_{gz::sim::kNullEntity}; + gz::sim::Entity linkEntity_{gz::sim::kNullEntity}; - transport::NodePtr node_; - transport::SubscriberPtr cmd_vel_sub_; + gz::transport::Node node_; double thrust_{0.0}; double yaw_rate_{0.0}; std::mutex cmd_vel_mutex_; }; - -GZ_REGISTER_MODEL_PLUGIN(UavFlightPlugin) - -} // namespace gazebo diff --git a/simulation/gazebo_uav_plugin/src/uav_flight_plugin.cpp b/simulation/gazebo_uav_plugin/src/uav_flight_plugin.cpp index d390828..d52f2ee 100644 --- a/simulation/gazebo_uav_plugin/src/uav_flight_plugin.cpp +++ b/simulation/gazebo_uav_plugin/src/uav_flight_plugin.cpp @@ -1,67 +1,48 @@ #include "uav_flight_plugin.h" #include "uav_flight_core.h" -#include - -#include - -namespace gazebo -{ - -UavFlightPlugin::UavFlightPlugin() -: model_(nullptr) -{ -} - -UavFlightPlugin::~UavFlightPlugin() -{ - /* update_connection_ RAII destructor disconnects the WorldUpdateBegin - * signal automatically. */ -} - -void UavFlightPlugin::Load(physics::ModelPtr model, sdf::ElementPtr /*sdf*/) +#include +#include +#include +#include + +#include +#include + +void UavFlightPlugin::Configure( + const gz::sim::Entity &entity, + const std::shared_ptr &, + gz::sim::EntityComponentManager &ecm, + gz::sim::EventManager &) { - if (!model) - { - gzerr << "[UavFlightPlugin] Load called with null model pointer\n"; - return; + entity_ = entity; + gz::sim::Model model(entity); + + /* Use the first link as the base link for force and torque application. + * Phase 42+ will replace this with per-rotor entity selection. */ + const std::vector links = model.Links(ecm); + if (!links.empty()) { + linkEntity_ = links.front(); + ecm.CreateComponent(linkEntity_, + gz::sim::components::ExternalWorldWrenchCmd()); + } else { + gzerr << "[UavFlightPlugin] No links found on model — forces will not be applied\n"; } - model_ = model; + const std::string modelName = model.Name(ecm); + const std::string topic = "/model/" + modelName + "/cmd_vel"; + node_.Subscribe(topic, &UavFlightPlugin::OnCmdVel, this); - node_ = transport::NodePtr(new transport::Node()); - node_->Init(); - const std::string topic = std::string("~/") + model_->GetName() + "/cmd_vel"; - cmd_vel_sub_ = node_->Subscribe(topic, &UavFlightPlugin::OnCmdVel, this); - - update_connection_ = event::Events::ConnectWorldUpdateBegin( - std::bind(&UavFlightPlugin::OnUpdate, this)); - - gzmsg << "[UavFlightPlugin] Loaded on model: " << model_->GetName() + gzmsg << "[UavFlightPlugin] Loaded on model: " << modelName << " — cmd_vel topic: " << topic << "\n"; } -void UavFlightPlugin::Reset() +void UavFlightPlugin::PreUpdate( + const gz::sim::UpdateInfo &, + gz::sim::EntityComponentManager &ecm) { - std::lock_guard lock(cmd_vel_mutex_); - thrust_ = 0.0; - yaw_rate_ = 0.0; -} - -void UavFlightPlugin::OnCmdVel(ConstTwistPtr &msg) -{ - std::lock_guard lock(cmd_vel_mutex_); - /* linear.z is mapped to collective thrust; angular.z to yaw rate. */ - thrust_ = msg->linear().z(); - yaw_rate_ = msg->angular().z(); -} - -void UavFlightPlugin::OnUpdate() -{ - if (!model_) - { + if (linkEntity_ == gz::sim::kNullEntity) return; - } double thrust = 0.0; double yaw_rate = 0.0; @@ -71,19 +52,31 @@ void UavFlightPlugin::OnUpdate() yaw_rate = yaw_rate_; } - /* Apply collective thrust as an upward body-frame force. The rotor - * joints are used in a full implementation; for Phase 38 we apply - * the net force directly to the base link so the UAV is controllable - * in simulation before per-rotor torque modeling lands. */ - auto links = model_->GetLinks(); - if (!links.empty()) - { - const auto cmd = gazebo_uav::compute_force_cmd(thrust, yaw_rate); - auto base = links.front(); - /* Force in world Z; torque about world Z for yaw. */ - base->AddRelativeForce(math::Vector3(0.0, 0.0, cmd.force_z)); - base->AddRelativeTorque(math::Vector3(0.0, 0.0, cmd.torque_z)); + /* Apply collective thrust as world-frame +Z force; yaw torque about world + * +Z. Forces are in world frame — appropriate for a hover-stable UAV at + * small angles. Phase 42+ replaces with body-frame per-rotor wrench. */ + const auto cmd = gazebo_uav::compute_force_cmd(thrust, yaw_rate); + + auto *wrenchCmd = ecm.Component(linkEntity_); + if (wrenchCmd) { + gz::msgs::Wrench wrench; + wrench.mutable_force()->set_x(0.0); + wrench.mutable_force()->set_y(0.0); + wrench.mutable_force()->set_z(cmd.force_z); + wrench.mutable_torque()->set_x(0.0); + wrench.mutable_torque()->set_y(0.0); + wrench.mutable_torque()->set_z(cmd.torque_z); + wrenchCmd->Data() = wrench; } } -} // namespace gazebo +void UavFlightPlugin::OnCmdVel(const gz::msgs::Twist &msg) +{ + std::lock_guard lock(cmd_vel_mutex_); + /* linear.z is mapped to collective thrust; angular.z to yaw rate. */ + thrust_ = msg.linear().z(); + yaw_rate_ = msg.angular().z(); +} + +GZ_ADD_PLUGIN(UavFlightPlugin, gz::sim::System, + gz::sim::ISystemConfigure, gz::sim::ISystemPreUpdate) diff --git a/simulation/gazebo_world_plugin/CMakeLists.txt b/simulation/gazebo_world_plugin/CMakeLists.txt index 18d4c88..cc7e891 100644 --- a/simulation/gazebo_world_plugin/CMakeLists.txt +++ b/simulation/gazebo_world_plugin/CMakeLists.txt @@ -15,25 +15,22 @@ if(BUILD_TESTING) endif() # ── Gazebo shared library ───────────────────────────────────────────────────── -find_package(gazebo QUIET) +find_package(gz-sim8 QUIET) +find_package(gz-plugin2 QUIET) -if(NOT gazebo_FOUND) - message(WARNING "Gazebo not found — world environment plugin will not be built.") +if(NOT gz-sim8_FOUND) + message(WARNING "gz-sim8 not found — world environment plugin will not be built.") return() endif() -include_directories( - include - ${GAZEBO_INCLUDE_DIRS} -) -link_directories(${GAZEBO_LIBRARY_DIRS}) -list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}") - add_library(world_environment_plugin SHARED src/world_environment_plugin.cpp ) target_include_directories(world_environment_plugin PRIVATE include) target_compile_options(world_environment_plugin PRIVATE -Wall -Wextra -Werror) -target_link_libraries(world_environment_plugin ${GAZEBO_LIBRARIES}) +target_link_libraries(world_environment_plugin + gz-sim8::gz-sim8 + gz-plugin2::gz-plugin2 +) install(TARGETS world_environment_plugin DESTINATION lib) diff --git a/simulation/gazebo_world_plugin/include/world_environment_plugin.h b/simulation/gazebo_world_plugin/include/world_environment_plugin.h index 0976ba9..5e0de14 100644 --- a/simulation/gazebo_world_plugin/include/world_environment_plugin.h +++ b/simulation/gazebo_world_plugin/include/world_environment_plugin.h @@ -1,42 +1,38 @@ #pragma once -#include -#include -#include +#include +#include +#include -namespace gazebo -{ +#include /** - * WorldEnvironmentPlugin — Gazebo WorldPlugin for the Mars-surrogate environment. + * WorldEnvironmentPlugin — Gazebo Harmonic system plugin for the Mars-surrogate world. * * Attach to the world SDF element: * * * Responsibilities: - * - Logs environment properties (gravity, wind, time step) at startup. - * - Forwards simulation-time ticks for synchronisation with the TAI authority - * (clock_link_model); actual clock authority is a separate container per - * docs/architecture/08-timing-and-clocks.md §5.2. - * - Emits periodic env telemetry (stub; wired to ICD-sim-fsw in Phase 39+). + * - Logs gravity, world name, and physics step size at startup. + * - Emits periodic heartbeat ticks (Phase 39+ wires to ICD-sim-fsw sideband SPP). + * - Does NOT set time authority — that is a separate clock_link_model container + * per docs/architecture/08-timing-and-clocks.md §5.2. */ -class WorldEnvironmentPlugin : public WorldPlugin +class WorldEnvironmentPlugin : + public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPostUpdate { public: - WorldEnvironmentPlugin(); - ~WorldEnvironmentPlugin() override; + void Configure(const gz::sim::Entity &entity, + const std::shared_ptr &sdf, + gz::sim::EntityComponentManager &ecm, + gz::sim::EventManager &eventMgr) override; - void Load(physics::WorldPtr world, sdf::ElementPtr sdf) override; + void PostUpdate(const gz::sim::UpdateInfo &info, + const gz::sim::EntityComponentManager &ecm) override; private: - void OnUpdate(); - - physics::WorldPtr world_; - event::ConnectionPtr update_connection_; - + gz::sim::Entity entity_{gz::sim::kNullEntity}; uint64_t tick_count_{0U}; }; - -GZ_REGISTER_WORLD_PLUGIN(WorldEnvironmentPlugin) - -} // namespace gazebo diff --git a/simulation/gazebo_world_plugin/src/world_environment_plugin.cpp b/simulation/gazebo_world_plugin/src/world_environment_plugin.cpp index dd7978e..0691bde 100644 --- a/simulation/gazebo_world_plugin/src/world_environment_plugin.cpp +++ b/simulation/gazebo_world_plugin/src/world_environment_plugin.cpp @@ -1,60 +1,41 @@ #include "world_environment_plugin.h" #include "world_environment_core.h" -#include - -namespace gazebo -{ - -WorldEnvironmentPlugin::WorldEnvironmentPlugin() -: world_(nullptr) +#include +#include + +void WorldEnvironmentPlugin::Configure( + const gz::sim::Entity &entity, + const std::shared_ptr &, + gz::sim::EntityComponentManager &ecm, + gz::sim::EventManager &) { -} + entity_ = entity; + gz::sim::World world(entity); -WorldEnvironmentPlugin::~WorldEnvironmentPlugin() -{ - /* update_connection_ RAII destructor disconnects the signal automatically. */ -} + const std::string name = world.Name(ecm); + const auto gravOpt = world.Gravity(ecm); -void WorldEnvironmentPlugin::Load(physics::WorldPtr world, sdf::ElementPtr /*sdf*/) -{ - if (!world) - { - gzerr << "[WorldEnvironmentPlugin] Load called with null world pointer\n"; - return; + gzmsg << "[WorldEnvironmentPlugin] Loaded world: " << name << "\n"; + if (gravOpt) { + gzmsg << " gravity: (" << gravOpt->X() << ", " + << gravOpt->Y() << ", " + << gravOpt->Z() << ") m/s²\n"; } - - world_ = world; - - update_connection_ = event::Events::ConnectWorldUpdateBegin( - std::bind(&WorldEnvironmentPlugin::OnUpdate, this)); - - /* Log environment properties at startup for verification (Phase A gate). */ - const auto gravity = world_->Gravity(); - gzmsg << "[WorldEnvironmentPlugin] Loaded world: " << world_->Name() << "\n" - << " gravity : (" << gravity.X() << ", " - << gravity.Y() << ", " - << gravity.Z() << ") m/s²\n" - << " max_step_size: " << world_->Physics()->GetMaxStepSize() << " s\n"; } -void WorldEnvironmentPlugin::OnUpdate() +void WorldEnvironmentPlugin::PostUpdate( + const gz::sim::UpdateInfo &, + const gz::sim::EntityComponentManager &) { - if (!world_) - { - return; - } - ++tick_count_; - if (gazebo_world::should_emit_heartbeat(tick_count_)) - { + if (gazebo_world::should_emit_heartbeat(tick_count_)) { /* Periodic heartbeat — Phase 39+ will replace this with sideband * SPP emission to the FSW sim_adapter app. */ - gzmsg << "[WorldEnvironmentPlugin] sim_time=" - << world_->SimTime().Double() << " s" - << " tick=" << tick_count_ << "\n"; + gzmsg << "[WorldEnvironmentPlugin] tick=" << tick_count_ << "\n"; } } -} // namespace gazebo +GZ_ADD_PLUGIN(WorldEnvironmentPlugin, gz::sim::System, + gz::sim::ISystemConfigure, gz::sim::ISystemPostUpdate)