Since we have a field relative swerve drive on ROS, figured I’d comment on how we did things.
On the joystick end, it is kinda weird because we have to support both a real DS and simulated joysticks. For the real DS, we have code in our hardware interface to periodically read from the DS and publish a joystick message. That’s fairly straightforward. For the sim version, we hook up the sim hardware interface to read from a ROS joystick input node and publish to the sim interface. The sim interface then calls the various HALSIM joystick set code, which is then read by the common part of the hardware interface code as if it were just like a real DS input.
In either case, we have a joystick message out, which goes to what we call the teleop node. That’s kind of a combo of your layer 2 and 3. We shape the raw stick inputs - ramping, power curves, deadband, etc. This is also where we apply the field centric offset. That comes from a node which reads from our IMU and republishes a sensor_msgs/Imu Documentation, rotated by an amount needed to correctly set 0 heading to be away from our driver. There’s a service call we make to the node which sets zero, and this captures a z angle offset which is applied to every subsequent imu message passed through the node.
The teleop node publishes a Twist message to a software mux. This is set up to take inputs from multiple sources of Twist messages - mainly teleop nodes and several different auto sequencing nodes. The mux picks the highest priority message and passes it through to the swerve drive controller.
The swerve drive controller converts robot velocity commands (both strafe and rotation) into motor commands. It also reads encoder values to calculate odometery (which in ROS is a nav_msgs/Odometry Documentation). And it also published a TwistStamped so other nodes can know what it is doing (useful for things like localization).
The way ROS does hardware control is it sets up a read - update - write loop. read and write are generic - they know that e.g. motors exist, but not what they’re used for. Each time read is called, the generic part of the hardware interface reads from all of the configured hardware and stores the state of each in buffers. Write does the opposite - takes commands queued up in command buffers and sends them to the actual hardware. This has two benefits - one, read and write are single threaded, and two, these buffers can just as easily be read/written to/from simulated devices as real hardware.
Each controller is a shared library, and there’s an interface to get devices by name, and then also to read and/or write the shared buffers accessed by the read() and write() steps of the control loop. There’s code in place to prevent multiple controllers from getting write access to hardware, but multiple controllers can read specific hardware’s state. Each controller’s update() function is called once per control loop, in arbitrary order (potentially in parallel as well), but due to the no-multiple-writers rule it works out.
Update generally reads a command from a topic (in our case, a Twist message) and converts it into individual commands to motors. Or pneumatics. Or digital outputs, whatever, same process. These writes can be actual motor commands, but we’re also set up to write e.g. config values to talons (switch modes, set PIDF, arb feed forward, most anything else the controller can handle). This is also where a layer of watchdogs are implemented - no Twist messages within 0.x seconds means the robot stops.