The homepage used to have an A* demo: a grid, some shelves, and a line that rerouted when you dropped a box. It worked, but it looked like a textbook exercise. I wanted something that showed more of a navigation stack, so I replaced it with a small robot that has never seen the floor it is on.
This is how that went, including the part that did not ship.
Mapping with a lidar
The robot has a simulated 360 degree lidar with 120 beams. It only knows the cells those beams have swept. Everything else is dark.
It plans its route through the dark as if the way were open, using the A* from my path planning simulator, and replans when a scan shows a wall on the route. The map is an occupancy grid. A cell only becomes a wall after hits from two separate scans, and beams passing through a cell count against it, so a box that is moved away is cleared again.
The route has a cost for running close to walls, the same idea as an inflated costmap in ROS. It keeps its distance where there is room and only squeezes past where there is no other way.
A planner that drives, not just a line
A* gives a route over cells. It does not say how fast to go or how sharply to turn. For that I added a dynamic window planner, the local planner that ROS navigation has used for years.
Twenty times a second it samples the speeds and turn rates the motors could reach in the next quarter second, rolls each one forward, and throws out any that would bring the robot's body too close to something the lidar sees. It drives the best of the rest, scored on progress along the route, staying near the route and keeping its distance.
Two details made it work. A candidate only has to be clear for as long as the robot would take to brake, plus a margin, otherwise tight turns past a shelf end get ruled out because the rolled out arc curls back into the shelf. And when nothing is worth driving for a moment, it backs off to make room and replans, which is what a real navigation stack's recovery behaviour does.
The bug I did not see
Early on I tested it the way I thought was thorough. I ran it for minutes, dropped boxes in front of it, and checked that its collision box never overlapped a wall. It never did.
Then I watched it and it was clearly brushing the shelving.
The collision box was 0.8 by 0.8. The robot on screen, with its wheels, was about 1.2 by 1.24. My tests were proving that a shape nobody could see never touched anything. When I measured the drawn shape instead, it overlapped something in 18% of frames.
Now there is one footprint. The planners, the collision check and the drawing all use the same rectangle. The lesson I keep relearning is to test the thing the user actually sees.
Trying AMCL and SLAM
Up to this point the robot knew exactly where it was. Real robots do not, so I tried to make it honest. Its wheels slipped, so its own sense of position drifted. A particle filter in the style of AMCL kept hundreds of guesses at its pose and kept the ones its scans agreed with, using a likelihood field as its sensor model. It built its map from that estimate, which makes it a simple SLAM. If you picked it up and put it somewhere else, it had to notice and find itself again.
I tested it headless: the same simulation running for simulated minutes at a time, measuring how far the estimate was from the truth, how often it was lost, and how many walls ended up in the wrong place. That found real bugs, one after another.
- The likelihood field only measured distance outside a wall. A guess that put beam endpoints deep inside a wall scored as well as the truth, so the estimate could slide into shelving.
- Beams that hit the edge of the arena landed just outside the grid and were penalised, so the correct pose looked wrong near the edges.
- The slip in my simulation persisted for seconds, but each guess got fresh noise every frame, which averages away. No guess could represent a steady slip. Giving each guess its own estimate of the slip fixed the drift.
- When a wall needed two scans to count, a cell hit once was marked as open floor. Beams landing on new walls then looked wrong, the map stopped updating, and the robot drifted.
- In frames where the robot stood still, the guesses were still being moved by the last speed command.
- People walking the aisles produced short readings. The filter tried to explain them by moving its pose until they lined up with a wall.
- A beam grazing a wall at a shallow angle, from a pose a few hundredths of a cell off, wrote a wall into the free cell beside it.
Each fix was measured before and after. Some made things worse and were reverted. By the end, normal running was confidently wrong or lost for a few percent of the time. After being picked up, it sometimes settled in an aisle that looked like the right one.
Why it did not ship
A few percent sounds small. On a homepage it means a visitor will sometimes see stray walls appear, or the robot sitting lost, or driving with total confidence from the wrong place. A demo that glitches says less about me than a smaller one that works.
The real fixes are well known: loop closure, a proper scan matcher, or a prior map of the building that the robot localises against while only mapping what moves. Each is a project of its own. The particle filter and SLAM version is kept on a branch for when I build that properly.
The people walking came out too. The only way I had made the robot ignore them in its map was to let the simulation tell it which readings were people, which a real robot cannot know.
What shipped
The robot knows its pose and has to discover everything else. A few changes made the rest solid.
- Each lidar return is taken just short of the surface it hit, so clearance is never overestimated.
- The local planner also checks its footprint against walls it has already mapped. Lidar returns are points, and the exact corner of a box can fall between them. The map has the corner.
- The wall cell for a hit is the one the beam actually stopped in, not a guess past the end of the beam, which had been putting stray walls next to box corners.
- Exploration goals are only chosen if there is a route to them, and an exploration goal counts as reached within a cell, so a goal that turns out to sit against a box does not leave it circling.
- The robot is sized against the aisles the way a real one would be, so a gap between two corners is a gap it can use.
The headless check for the version on the homepage ran ten five-minute sessions twice. Across those 100 simulated minutes the robot never overlapped anything, the physics never had to stop a move the planners allowed, it never stalled, no wall was mapped in the wrong place, and it reached about fourteen goals every five minutes.
What I would carry forward
Measure what the person watching will see, not a proxy for it. Run the system far longer than you would watch it. Change one thing at a time and keep the numbers from before. And be willing to take out the impressive part when the simpler version is the one that is true every time.