Forklift Controller

Drives the simulated forklift. The forklift-controller service runs one ROS 2 node per truck, closed-loop-testing/forklift-controller/robot_controller.py, that reads the forklift’s odometry, steers toward the next pose on a stored route, and publishes cmd_vel to the Forklift Control Graph. The code comes from the forklift-controller image, not from a bind mount of the checkout, so source edits need up -d --build.

Role in the Loop

The controller is a stimulus generator. It keeps the forklift driving a fixed route so that perception always has a vehicle to detect, Behavior Analytics always has ROI and tripwire crossings to report, and the Safety Core always has a decision to make. Under PSF_APP=pxc or both it also obeys the Safety Core’s proximity decisions (see Proximity Decisions). Nothing else drives the forklift — character motion comes from the IRA behavior trees inside Isaac Sim (see Isaac Sim Configuration).

waypoints/<map>/<robot>.json      /safety/proximity/pair (pxc / both)
     │                                  │
     v                                  v
forklift-controller ── /forklift_b/cmd_vel ──> Control graph ──> articulation
     ^                                                               │
     │                                                            physics
     └────────── /forklift_b/odom <── Odometry graph <────────────────┘

The controller publishes in the ROS twist convention; the sign flips for the forklift asset’s vehicle frame are applied on the Isaac Sim side (reverse_logic in the model block of robots.yaml — see Forklift Control Graph).

ROS 2 Interface

One node per robot, named <robot>_controller (forklift_b_controller for the default truck), so two controllers in one ROS domain do not collide. The robot is the service’s ROBOT_ID, which must name a robot in the scenario’s fleet file. Every topic below uses RELIABLE QoS with depth 10. The drive and feedback topic names come from the robot’s own block in the fleet file — control.cmd_vel_topic and odometry.odom_topic, the same keys Isaac Sim builds its graphs from — so the two sides cannot disagree.

The state message carries robot_id, state, speed_factor, pose_idx, total_poses, and completed, plus proximity, proximity_separation_m and proximity_fault — the cheapest way to watch progress without parsing the log. A latched nav_msgs/Path publisher on /<robot>/planned_path is also created but never written to, so the route cannot be visualized from ROS.

Route Following

The route is loaded once at startup from the waypoint file. Each pose is converted from the file’s world coordinates into the odometry frame by subtracting origin. With pose inversion on (the default), the route is then rotated 180° about that origin: x and y are negated and π is added to theta. Separately, drive.heading_offset — 180° by default — is added to the yaw read from odometry. Both defaults exist for the shipped forklift asset (see robot_front in Forklift Odometry Graph).

Advancing Along the Route

A pose is reached when the forklift is within 0.35 m of it, or within drive.end_tolerance for the last drive.end_pose_count poses of the route. The index then advances by one.

Steering is pure pursuit: the controller walks forward from the current pose, up to 15 poses and never across a segment boundary, until it has accumulated max(0.8, drive.speed × 1.5) metres of path, and steers at that carrot. Angular velocity is the heading error times 1.2, clamped to drive.angular_speed. On a segment marked reverse the desired heading is rotated by π and linear.x is negative.

Linear speed is then shaped — scaled by max(0.4, distance / 0.5) inside 0.5 m of the target, by 0.4 above 30° of heading error and by 0.2 above 60°, then floored at 0.15 m/s in magnitude so the forklift keeps steering authority.

At the end of the route, drive.loop: true (the shipped robots.yaml) wraps the index back to zero and logs Loop: Restarting path. With looping off the controller publishes a stop, logs ✓ Path completed!, and transitions to IDLE — the scene keeps rendering but stops producing crossings.

Overshoot Recovery

Following live odometry means the forklift can pass a pose it never came close enough to retire, and then circle back for it. Three guards catch that.

Skip ahead: when the target is more than 1 m away and at least three poses remain after the current target, the controller scans up to 9 poses ahead within the same segment and jumps to the nearest one — ⏭️ SKIP AHEAD. Occasional lines are normal; a burst of them means the route is tighter than the forklift can turn at drive.speed.

Passed-pose skip: when the distance to the target has grown for more than 20 control ticks (roughly one second), the closest approach was under 0.5 m, and the forklift is now more than 0.3 m past that closest approach, the pose is retired — ⏭️ SKIPPING pose. The last pose of the route is excluded from this skip. The distance printed in this line always reads infm — the pose index is the useful part.

Spiral timeout: at the final pose only, when it has been the target for longer than drive.spiral_timeout and the forklift is within 2 m, the controller declares the route complete — 🔄 SPIRAL DETECTED. This ends the run: it stops and goes IDLE even with looping on.

Simulation Stop, Play, and Reset

The controller tracks /clock so that an operator driving the timeline in the Isaac Sim UI does not leave it commanding a frozen scene:

• No /clock for 2 seconds — publishes a stop and holds, logging ⏸️ SIM STOPPED (no /clock) - pausing robot. It resumes when the clock does.
• Simulation time jumping backwards by more than 0.5 s — restarts the route, logging 🔄 SIM RESET detected. Only the backwards jump restarts the route: a pause and resume that leaves simulation time where it was resumes from the current pose.
• An odometry position jump of more than 5 m — restarts the route, logging ⚡ ODOM JUMP detected.

A restart does not return to pose 0. The controller scores every pose by distance plus a heading penalty of 3 m per radian and continues from the best match, logging 🔁 Path reset.

State Machine and Speed Scaling

Published linear speed is drive.speed times the state’s factor times the command’s speed_factor (clamped to 0.0–1.0, 1.0 when absent), before the near-target and heading-error shaping above, and before the proximity limit (Proximity Decisions). In IDLE and PAUSED nothing is published at all: the controller emits one zero Twist on entering the state and then stops feeding the loop.

The node transitions to MOVING as soon as it starts, with no command and no handshake with the simulation. 🔄 State: IDLE → MOVING therefore appears in every startup log and is not evidence that the forklift is moving — an advancing 📍 pose= line is.

Safety Commands

/safety/command is the topic the communication layer republishes each safety decision on. The controller subscribes to it and reads three fields out of the JSON payload: robot_id (optional; a message naming a different robot is ignored), command, and speed_factor.

The string verbs in the state table above are what move it, and the bundled helper sends them. It accepts proceed, stop, slow, idle, go, pause, and resume, with --speed to override the speed factor and --robot to target one controller:

# run inside the container, which carries the helper and the ROS 2 environment
docker exec forklift-controller bash -lc \
  "source /opt/ros/jazzy/setup.bash && python3 /app/test/send_command.py slow"

Expected output:

[INFO] [...] [forklift_b_controller]: 📥 [forklift_b] Command: slow | speed_factor=0.30
[INFO] [...] [forklift_b_controller]: 🔄 State: MOVING → SLOW (speed_factor=0.30)
[INFO] [...] [forklift_b_controller]: 📍 pose=9 ▶FWD dist=0.71m cmd=(0.15,0.08) robot=(5.02,-13.37,178°)

speed_factor=0.30: the helper’s slow default, applied on top of the SLOW state’s own 0.3.

cmd=(0.15,…): the linear value has hit the 0.15 m/s floor — the two 0.3 factors compound.

reset is handled before the state machine: it re-snaps to the nearest heading-compatible pose, exactly as after a simulation reset, and starts moving again.

Proximity Decisions

With PSF_APP=pxc or both, the communication layer republishes each proximity decision of the Safety Core on /safety/proximity/pair, and the controller limits the Twist it is about to publish:

• STOP — zero linear and angular speed.
• REDUCE — linear speed capped at proximity.reduce_speed; angular speed is scaled by the same ratio, so a truck slowed mid-corner stays on the corner it was driving.
• NORMAL — the route speed.

A STOP holds proximity.stop_hold_s past the last packet asking for one, and a REDUCE proximity.reduce_hold_s. Silence keeps the last level: no person in view means no decision, not NORMAL. The link itself is watched: the decision maker heartbeats every 5 s, and proximity.stale_s with no message at all stops the truck with fault psf_link_stale until the next decision. A safety-critical packet without a level (an SDM fault) also stops the truck.

The Safety Core names no robot in a proximity decision, so every truck on one pair_topic obeys every decision. The controller logs a WARNING when more than one truck shares the topic; give a second truck its own proximity.pair_topic or enabled: false.

Waypoint File

Each controller reads one route, /app/waypoints/<map id>/<ROBOT_ID>.json. The whole closed-loop-testing/forklift-controller/waypoints/ tree is bind-mounted read-only at /app/waypoints, and the map id comes from the scenario’s waypoints: entry in scenarios.yaml (warehouse_20x20 for warehouse_20x20_1fl), or from WAYPOINTS_MAP for a single run. Coordinates are metres in that scene’s world frame, so the wrong map drives a lane that was never validated in that warehouse. Author routes with the Forklift Waypoint Generator on the Isaac Sim Configuration page, which also documents the export format.

Three top-level keys are read:

• origin — world_x and world_y, the zero of the odometry frame. Every pose is expressed relative to it. It must be where the truck spawns (spawn.position in robots.yaml); deployments/scripts/preflight.py compares the two before Isaac Sim boots.
• poses — the interpolated route: x and y in world metres, theta in radians.
• segments — run-length ranges over poses, pose_count entries each, whose reverse flag drives that stretch backwards. The shipped file has two: a forward leg out and a reverse leg back.

Everything else in the file is authoring metadata and is not read — including the waypoints array itself and the velocity field on each of its entries. Route speed comes from drive.speed and is uniform; there is no per-waypoint or per-segment speed.

pose_count is walked cumulatively and a pose past the last range falls back to forward travel, so a hand-edited file whose pose_count values sum to less than the number of poses drives its trailing reverse stretch forwards with no warning.

Configuration

How a truck drives lives in the fleet file, not in the environment: one environment variable would set every truck in the fleet at once. Each knob resolves as a launch flag (launch_controller.sh, docker_run.sh), else the robot’s drive: block, else its model’s drive: block, else the built-in default. The controller prints the value and source of each one at startup ([drive]).

drive: refuses unknown keys, so a key indented one level too far is an error, not a silent default.

The service itself reads four variables: ROBOT_ID (set on the compose service, forklift_b), and SCENARIO, WAYPOINTS_MAP and PSF_APP from the profile env file.

To follow a route of your own in the same warehouse, put one file per truck under a new directory and select it for the run:

# closed-loop-testing/forklift-controller/waypoints/my_routes/forklift_b.json
# in deployments/profiles/sil.env:
WAYPOINTS_MAP=my_routes

Recreate the service:

cd deployments && docker compose --env-file profiles/sil.env up -d --force-recreate forklift-controller

docker logs forklift-controller should show [waypoints] /app/waypoints/my_routes/forklift_b.json and Loaded path: <N> poses with the pose count of your file.

Logs to Watch

docker logs --tail 20 forklift-controller

Expected output:

[scenario] warehouse_20x20_1fl: robots=robots.yaml waypoints=warehouse_20x20
[drive] angular_speed=0.4(robots.yaml) end_pose_count=5(default) ... speed=1.5(robots.yaml) spiral_timeout=10.0(default)
[topics] cmd_vel=/forklift_b/cmd_vel odom=/forklift_b/odom proximity_pair=/safety/proximity/pair proximity_state=/forklift_b/proximity/state
[waypoints] /app/waypoints/warehouse_20x20/forklift_b.json
[INFO] [...] [forklift_b_controller]: Loaded path: 41 poses
[INFO] [...] [forklift_b_controller]: Odom received: (1.02, -13.41, 179.8°)
[INFO] [...] [forklift_b_controller]: 📍 pose=7 ▶FWD dist=0.62m cmd=(1.50,0.12) robot=(4.21,-13.38,178°)

[scenario] / [drive] / [topics] / [waypoints]: what the controller resolved, and where each drive value came from — the first lines to read when a run behaves oddly.

Loaded path: 41 poses: the waypoint file parsed, printed once at startup.

Odom received: the first odometry sample. Until it appears the controller is running but publishing no cmd_vel.

📍 pose=: printed every two seconds while moving. robot= coordinates that change from line to line are the only proof that the forklift is driving.

🔄 SEGMENT TRANSITION: a segment boundary, reading FORWARD→REVERSE or REVERSE→FORWARD. On the shipped route the first one is the forklift backing out of the trailer.

🦺 PXC [forklift_b] normal -> reduce_speed: a change of the proximity level applied (pxc / both), with the separation and the resulting speed.

Limitations

Decisions are not addressed to a truck

Cause: /safety/command is global, and the Safety Core names no robot in a proximity decision, so every controller in the ROS domain acts on every command, and every truck on one pair_topic obeys every proximity decision.

Workarounds: name robot_id in every command message; give a second truck its own proximity.pair_topic or proximity.enabled: false; or run one controller per ROS domain. Two-truck runs use the multi-robot compose profile (SCENARIO=warehouse_40x20), which starts forklift-controller-b2 for forklift_b2.

Up is not proof of motion

Cause: the service has no healthcheck and no depends_on, and the node starts, subscribes, and waits regardless of whether odometry ever arrives. A count of running containers is therefore not a check that the loop is closed.

Workarounds: gate on the 📍 pose= line advancing, or on /<robot_id>/state reporting a rising pose_idx.

Next Steps

• Isaac Sim Action Graphs — how cmd_vel becomes wheel commands, and the odometry that feeds back
• Isaac Sim Configuration — the scene, the robots.yaml robot block, scenarios.yaml, and the waypoint generator
• Extending the Fleet — add a second truck, a different drive type, or a new scene
• SIL Overview — MUTE and UNMUTE, the vocabulary the safety decision is reported in
• SIL — 2D Perception — deploy the stack this service runs in