Autonomy that finds its own way

At the European Rover Challenge, the navigation task has a version that pays the most points and asks the most of you: reach five checkpoints on the Marsyard autonomously, with no video feedback to the operator. GPS is off the table. So the rover has to answer three questions on its own — where am I, where do I go, and how do I get there — using nothing but its own sensors.

Here is how our stack strings those three answers together.

Where am I: perception and localisation

Localisation is a fusion problem. No single sensor is trustworthy enough on loose regolith, so we combine several, each covering the others’ blind spots:

• Wheel odometry reads the drive and steering encoders over TCP and runs a four-wheel forward-kinematics model, integrating the resulting body twist with RK4 to produce a pose. It is smooth and high-rate, but it drifts as wheels slip.
• The Hipnuc CH110 IMU supplies orientation, angular velocity and acceleration to anchor heading between updates.
• ArUco markers give the drift-free fix. The pipeline detects 5×5 markers with OpenCV, runs solvePnP on each one, then folds several detections into a single global pose with a multi-marker Levenberg–Marquardt least-squares optimisation. More markers in view means a tighter estimate.
• The ZED stereo camera and the Blickfeld Qb2 solid-state LiDAR round out the picture — dense depth and a 3D point cloud for perceiving the terrain around the rover.

A dedicated transform node applies the rotation and translation offsets between the IMU, wheel and rover frames, and the fused result is published on /odomrover as standard nav_msgs/Odometry. That single topic is the ground truth everything downstream trusts.

Lesson learned the hard way: the gyroscope has to sit parallel to the rover and the ground. Mount it at an angle and the readings are quietly useless — and localisation built on top of them inherits every degree of that error.

Where do I go: planning over a cost map

Waypoints arrive as a geometry_msgs/PoseArray on /set_waypoints. A planning service turns them into a route by running Dijkstra’s shortest-path algorithm over a 2D cost map of the Marsyard.

The map is a NumPy array — roughly 2892×2892 pixels at 66 pixels per metre — where each cell carries a traversal cost. Anything above a cost threshold of 200 is treated as blocked and simply never enters the search frontier, so the planner routes around rocks and slopes rather than charging through them. The service exposes exactly the handles the mission needs: set a start, set the waypoints, plan the path, and mark regions as blocked when we learn the terrain is worse than the map claimed.

Dijkstra is deliberate here. It is not the flashiest choice, but on a static cost map it is exhaustive and predictable — it returns the genuinely lowest-cost path or none at all, and “predictable” is worth a great deal when the operator cannot see what the rover is doing.

How do I get there: a behaviour tree at the wheel

Executing the path is the job of fhnw_waypoint_navigator, built on BehaviorTree.CPP. A behaviour tree keeps the driving logic legible: every tick, the tree walks a small, explicit sequence of checks and actions rather than hiding the state machine in a tangle of if statements.

The main tree, roughly in order, does this each cycle:

1. GoalReached? — are we at the final waypoint? If so, stop.
2. EnsureMayDrive — is the navigation state actually clear to move, and not waiting or interrupted?
3. SetPathPoint — pick the current target point off the planned path.
4. PathPointReached? — within 0.1 m of it? Advance the index.
5. NavigateToPathpoint — compute and publish a drive command.
6. CheckStillOnRoute — are we within 0.3 m of the planned path?
7. TriggerRecalculatePath — if we have drifted off route, ask the planner for a new path.

That last pair is the important bit. The rover is not blindly replaying a route; it is continuously checking itself against the plan and replanning when reality diverges — a wheel slips, a rock shifts, the pose estimate jumps as a new marker comes into view.

The drive commands themselves are shaped for a machine that steers all four wheels. The navigator publishes a DrivingInputControlMsg — turning radius, velocity and crab angle — on /drivetrain_input_commands. It cruises at 0.5 m/s when the target is far, eases to 0.2 m/s as it closes in, and if the heading error to the next point exceeds 15° it stops and turns in place rather than swinging out on a wide arc it has no room for.

Practising before Poland

None of this gets tuned on the real Marsyard for the first time. We iterate in simulation — MuJoCo for fast experiments and Gazebo for fuller scenes — which lets us throw bad odometry, missing markers and awkward waypoint geometries at the stack long before the rover leaves the lab. (One honest caveat: our older Gazebo Classic drive plugin does not build on ROS 2 Jazzy’s Gz Sim, so that corner of the sim tooling is mid-migration.)

Autonomy is never really “done.” But when the rover sets off toward a checkpoint it has never seen, plans its own way around a rock, and quietly corrects course when it drifts — with the operator watching a blank video feed — that is the whole stack, from photons to wheel commands, doing exactly what it was built to do.