Snappy

Snappy: Software Architecture

High-level Architecture

Everything on this page runs on the Jetson under ROS 2 Humble, inside Docker. The boards (ESCs, power, BMS) have their own firmware and are covered in Hardware.

flowchart TD
    Drivers["Sensors and drivers"] --> StateEst["State estimation"]
    Cameras["Cameras"] --> Vision["Vision"]
    StateEst --> Controls["Controls"]
    StateEst --> Planner["Mission planner"]
    Vision --> Planner
    Planner --> Controls
    Controls --> Allocator["Thruster allocator"]
    Allocator --> CanDriver["Thruster driver node"]
    CanDriver --> ESC["ESC boards"]
    Safety["Safety"] --> Controls
    Safety --> CanDriver
    Kill["Kill switch (hardwired)"] --> ESC

Loading

How the loop closes

flowchart LR
    subgraph Jetson["Software running on the Jetson (ROS 2 nodes)"]
        Vision["Vision<br/>finds objects"]
        StateEst["State estimation<br/>ESKF: where am I?"]
        Planner["Mission planner<br/>what next?"]
        Controls["Controls<br/>PID: how hard to push?"]
        Allocator["Thruster allocator<br/>which thruster does what?"]
        Driver["Thruster driver node"]
    end

    Cameras["Cameras<br/>D455 front, D405 bottom"] --> Vision
    Nav["IMU, DVL, depth sensor"] --> StateEst

    Vision -- "objects seen" --> Planner
    StateEst -- "position, speed, heading" --> Planner
    StateEst -- "position, speed, heading" --> Controls
    Planner -- "next move" --> Controls
    Controls -- "progress, done or failed" --> Planner

    Controls -- "force and torque wanted" --> Allocator
    Allocator -- "8 thruster forces" --> Driver
    Driver -- "commands" --> Boards["Thruster boards<br/>(STM32)"]
    Boards --> Thrusters["8 thrusters"]
    Thrusters --> Water(["The sub moves<br/>through the water"])
    Water -. "sensors feel the motion" .-> Nav
    Water -. "the view changes" .-> Cameras

Loading

The fast loop (many times a second) keeps the sub on target:

1. The IMU, DVL and depth sensor measure how the sub is moving.
2. State estimation (ESKF) combines them into one answer: position, speed and heading.
3. Controls compares that with the target and the PID loops work out the push needed, as a force and a torque on the body.
4. The thruster allocator splits that push across the eight thrusters.
5. The thruster driver node sends the commands to the STM32 thruster boards, which spin the thrusters.
6. The sub moves. The sensors feel it, and we are back at step 1.

The slow loop (once per move, seconds to minutes) runs the mission:

1. The mission planner picks the next move, such as "go forward 3 m" or "line up with the gate", using the sub's position and what vision sees.
2. It sends that one move to Controls.
3. The fast loop runs until the sub has stayed on target long enough, or the move times out.
4. Controls reports done or failed.
5. The mission planner picks the next move: carry on, retry, or skip.

The diagram above shows who talks to whom. This one shows the data going all the way round.

Subsystem	Package	What we use
Sensors and drivers	snappy_drivers	One ROS node per device
State estimation	snappy_estimation	Error-state Kalman filter (ESKF)
Controls	snappy_control	Cascaded PID + thruster allocator
Mission planner	snappy_autonomy	BehaviorTree.CPP
Vision	snappy_computer_vision	Object detection on two RealSense cameras
Safety	snappy_safety	Arm state, timeouts, pre-dive checks
Simulation	separate repo	Stonefish
Shared messages	snappy_interfaces	Custom messages, services, actions

Sensors and drivers

What it does: reads each device and publishes its data as a ROS topic with a timestamp. No decision-making lives here.

What we use:

• Xsens IMU: orientation, angular rate, acceleration
• WaterLinked DVL: velocity over the pool floor
• Bar02 pressure sensor: depth, arriving over serial from a microcontroller
• Front camera: Intel RealSense D455 (colour + depth, has its own IMU)
• Bottom camera: Intel RealSense D405 (colour + depth, built for close range, no IMU)

Currently the depth driver only works on /dev/ttyUSB0, does not reconnect if unplugged, and publishes depth with no timestamp. This year, we should have every sensor message carry a timestamp. Drivers reconnect on their own. Device names are stable (udev rules), so nobody has to replug cables in a certain order.

Thruster driver node

The last piece of software before the hardware. It takes the eight thruster forces from the allocator and sends them to the ESC boards. It lives in snappy_drivers next to the depth and solenoid drivers.

Today there is no such node. The controller does this job itself: it publishes /motor_cmd as thrust percentages and the micro-ROS agent carries that straight to the Mower Board over serial. But this year we are getting rid of micro-ROS and switching to using CAN

What it will do:

1. Subscribe to /thrusters/command: eight forces in Newtons from the allocator.
2. Convert Newtons into whatever the ESC boards accept. This is the only place that conversion happens. Nobody else touches PWM, duty or percent.
3. Send the result over CAN using the message layout in the DBC file.
4. Read the boards' replies (current, voltage, faults) and publish them as /thrusters/status.
5. One safety check: send zero unless /safety/state says armed and is fresh.
6. Send zero when it shuts down. Never resend an old command.

State estimation

What it does: combines the IMU, DVL and depth sensor into one best guess of where the sub is, how fast it is moving and which way it points.

What we use: an error-state Kalman filter (ESKF). It tracks position, velocity, orientation and accelerometer bias. Orientation comes from the Xsens's own onboard filter; we do not integrate the gyro ourselves.

Today:

• The second IMU update is broken: its matrix sizes do not match (9 measurements, 3×3 noise matrix). Keep it turned off.
• DVL readings near zero are thrown away, so the filter never learns that the sub is sitting still.
• No check that rejects a wild measurement before it is fused.
• The output is a custom Pose message with no timestamp, so Controls cannot tell how old it is.

Frames: x forward, y right, z down. Details in Software Subsystems (Nodes).

Controls

What it does: takes the target from the mission planner and the current state from state estimation, and works out how hard each thruster pushes.

What we use:

• PID control. Today this is six ordinary PIDs, one per axis (x, y, z, roll, pitch, yaw).
• Thruster allocator. Converts the wanted force and torque on the body into eight thruster forces using the measured thruster positions and directions (pseudoinverse of the allocation matrix). If a thruster would exceed its limit, all thrusters scale down together so the direction of the push is kept.
• Units are SI everywhere: metres, radians, Newtons. Orientation is a quaternion between nodes; angles only in the mission file.

Topic/Data Flow

Interfaces