Write a driving node

A driving node reads sensors, decides a steering angle and speed, and publishes them on /drive. ackermann_mux passes them to the motor when no manual command is active. gap_follow is the template: copy its structure, its deadman check and its tests.

The contract

Steering is in radians, not degrees, and is limited by the servo range in vesc.yaml. Speed is in metres per second: 1.0 is a brisk walk.

Never publish past the mux, on /ackermann_cmd or /commands/*. That skips the manual override entirely.

The LB deadman check

Every driving node checks LB itself, as the first thing it does before publishing. This is a workspace policy, explained in the safety model. Copy the pattern from gap_follow_node’s joy_callback and _deadman_status in gap_follow_node.pyGitHub:

self.declare_parameter('joy_topic', '/joy')
self.declare_parameter('deadman_button', 4)      # LB
self.declare_parameter('joy_timeout_sec', 0.5)
self.declare_parameter('enable_deadman', True)   # never False on the car

self.joy_sub = self.create_subscription(Joy, self.joy_topic, self.joy_callback, 10)

def joy_callback(self, msg):
    self.last_joy_time = self.get_clock().now()
    self.joy_button_available = len(msg.buttons) > self.deadman_button
    self.deadman_held = self.joy_button_available and bool(msg.buttons[self.deadman_button])

def deadman_engaged(self):
    if not self.enable_deadman:
        return True
    if self.last_joy_time is None:                       # no /joy since start
        return False
    age = (self.get_clock().now() - self.last_joy_time).nanoseconds / 1e9
    if age >= self.joy_timeout_sec:                      # /joy has stopped
        return False
    return self.deadman_held                             # LB down right now

In the callback or timer that publishes, call deadman_engaged() first. If it returns False, publish speed 0 and return.

Each condition covers a different failure. last_joy_time is None covers the moment after launch, before any gamepad message has arrived. The age check covers the dangerous case: if joy_node dies or the gamepad loses power, no new message arrives and deadman_held keeps its last value, possibly True, indefinitely. With the age check the car stops 0.5 s later. That check never fires in normal use, which is why it is the one people delete when tidying code.

Run it

Start bringup, then the node. Do not start teleop_launch.py: its commands on /teleop always win, so the node’s /drive would never reach the motor.

ros2 launch f1tenth_stack bringup_launch.py            # terminal 1
ros2 launch <your_package> <your_node>_launch.py       # terminal 2
ros2 topic echo /drive                                 # terminal 3

The speed in terminal 3 must read 0.0 the moment LB is released. Any delay, or any non-zero speed with LB released, must be fixed before going further.

Test order

Do these in order without skipping a step. Each one catches faults the next would let reach the car.

1. Static check. Run the node with no driver stack at all, feeding it a recorded bag or the ROS simulator. Check the signs and units of /drive. Nothing can move.
2. Wheels off the ground. Bringup and the node, with the car on a stand. Check the steering and drive direction, and that releasing LB stops the wheels.
3. Floor, low speed, open space. One person at the battery switch.

Tests the node needs

Each driving node implements the LB check separately, so each needs its own four tests. Another node’s tests do not cover it. Assert on the command the node publishes, not on an internal flag.

What the tests look like

gap_follow’s own deadman tests, in test_gap_follow_node.pyGitHub, are the pattern to copy. A fixture builds a real node for each test and tears it down afterwards. It starts the node with its drive topic remapped to /test_only/drive, as the last argument so that no parameter file can override it, and checks the remap took effect before any test runs. Shortened, with some of its parameters left out:

TEST_ONLY_DRIVE = ['-p', 'drive_topic:=/test_only/drive']

@pytest.fixture
def node():
    rclpy.init(args=['--ros-args',
                     '-p', 'min_gap_distance:=2.0',
                     '-p', f'max_speed:={MAX_SPEED}',
                     *TEST_ONLY_DRIVE])
    n = GapFollowNode()
    assert n.drive_pub.topic_name == '/test_only/drive'
    yield n
    n.destroy_node()
    rclpy.shutdown()

Small helpers build the input messages and call the node’s callbacks directly, with no publisher and no waiting. _capture wraps the node’s publisher so the test can read every command it sent:

def _hold_deadman(node, held=True):
    msg = Joy()
    msg.buttons = [0] * (DEADMAN_BUTTON + 1)
    msg.buttons[DEADMAN_BUTTON] = 1 if held else 0
    node.joy_callback(msg)

def _capture(node):
    published = []
    original = node.drive_pub.publish
    node.drive_pub.publish = lambda msg: (published.append(msg), original(msg))
    return published

Each deny-path test then sets up one failure and asserts on the published command, not on a flag inside the node:

def test_refuses_to_drive_when_the_deadman_is_released(node):
    published = _capture(node)
    _ready(node)                        # LB held and /odom fresh
    _hold_deadman(node, held=False)     # then LB released
    node.scan_callback(_scan())
    assert published[-1].drive.speed == 0.0
    assert node.last_decision_state == 'deadman_released'

def test_stops_when_the_joy_stream_goes_stale(node):
    published = _capture(node)
    _ready(node)
    node.last_joy_time = node.get_clock().now() - Duration(
        seconds=node.joy_timeout_sec + 0.1)
    node.scan_callback(_scan())
    assert published[-1].drive.speed == 0.0
    assert node.last_decision_state == 'joy_stale'

The stale test moves the last /joy time back past the timeout instead of sleeping for half a second, so it runs instantly and cannot be flaky. Before calling a new test done, break the deadman check in the node, for example by making deadman_engaged() always return True, and watch the test fail. See Test quality standards.

A node test may set enable_deadman:=false only if the same node is also started with drive_topic:=/test_only/drive, and never in a test about the deadman itself. Without the remap, a test with the driver stack up publishes real commands into ackermann_mux. See Run the tests.

Publish drive intent

To explain the node’s decisions on the dashboard, publish /drive_intent after each command, following the safety rules for publishers.