03Singularity Hub
SLAM and A* navigation
This is the page where your robot starts to find its own way. First it draws a map of the place (SLAM). Then, when you click where it should go, it plans the shortest safe path on that map (A*) and drives there on its own. Everything here runs in Singularity 1.0 today. We will build it up one idea at a time.
What SLAM is
Imagine walking through a dark house with a torch. As you go, you draw the walls you see on a piece of paper, and at the same time you use that drawing to work out where you are standing. That is SLAM: Simultaneous Localization And Mapping. The robot draws the map and finds itself on it, at once.
Singularity’s SLAM works like slam_toolbox on a real ROS 2 robot: it matches each LiDAR scan to the map it has drawn so far. That is why it drifts much less than wheel odometry, which only counts wheel turns.
| What SLAM gives you | Simple Python | ROS 2 topic |
|---|---|---|
| The map | (see below) | /map |
| Where SLAM thinks the robot is | robot.slam_pose() | /pose |
| How much of the floor is mapped (0 to 1) | robot.map_explored() | — |
Turn SLAM on
There are three ways. Any one is enough:
- Map window: open Map in the top bar and press SLAM.
- Terminal: type
ros2 launch singularity slam.launch.pyand press Enter. - Lessons that need SLAM (Map with SLAM, Map a Building by Hand) switch it on for you.
In the Map window you will see the map grow: dark cells are walls, light cells are free floor, and the rest is not seen yet. The checkboxes show the LiDAR Scan, the Truth (where the robot really is) and the Odometry path. Fit zooms to the whole map.
Watch SLAM work
This program follows the right-hand wall round the Maze until 90% of the floor is mapped. Every 5 seconds it prints how far SLAM and wheel odometry are from the truth. Watch the odometry error grow while SLAM stays small.
# Watch SLAM work · world: Maze
# First press SLAM in the Map window (or type: ros2 launch singularity slam.launch.py).
# The robot follows the wall on its right, all the way round the maze. SLAM draws
# the map and keeps track of where the robot is. Every 5 seconds we compare SLAM and
# wheel odometry with the true position (which only a simulator can give you).
import math
def closest(center, width):
"""Closest LiDAR reading in a slice of the circle (degrees, 0 = ahead, 90 = left)."""
scan = robot.lidar()
return min(scan[a % 360] for a in range(center - width // 2, center + width // 2 + 1))
while robot.map_explored() is None: # None means SLAM is off
print("SLAM is off. Press SLAM in the Map window.")
robot.wait(1.0)
GAP = 0.35 # how far to stay from the right-hand wall (m)
next_report = 0
while robot.map_explored() < 0.9: # stop when 90% of the floor is on the map
front = closest(0, 40)
wall = min(closest(-90, 30), closest(-45, 20) * 0.7071)
if front < GAP + 0.1:
robot.turn_left(1.0) # wall ahead: turn left on the spot
elif wall > 1.0:
robot.drive(0.18, -0.9) # lost the wall (outside corner): arc right
else:
robot.drive(0.22, 2.5 * (GAP - wall)) # steer to keep the gap
robot.wait(0.1)
if robot.time() >= next_report: # print a report every 5 seconds
next_report += 5
sx, sy, _ = robot.slam_pose()
ox, oy, _ = robot.odometry()
tx, ty, _ = robot.true_pose()
print(f"explored {robot.map_explored():.0%} | SLAM is off by {math.hypot(sx - tx, sy - ty) * 100:.0f} cm"
f" | wheel odometry is off by {math.hypot(ox - tx, oy - ty) * 100:.0f} cm")
robot.stop()
print("Done:", round(robot.map_explored() * 100), "% of the floor is on the map.")The map lives while SLAM is on
Singularity 1.0 has no “save map” button. The map is kept in memory and updated about once a second for as long as SLAM is on. That is all A* needs: it plans on the live map, and plans again when the map grows. Here is what keeps the map and what clears it:
| You do this | The map |
|---|---|
| Keep driving (by hand or by program) | keeps growing |
| Press Run, in the default “Start over” mode | starts fresh, because the robot goes back to its start spot |
| Press Run with “Keep going from here” (World tab) | is kept, so you can map by hand and then run a program on that map |
| Press Reset, change world or robot | starts fresh |
| Turn SLAM off | is cleared |
To keep a map for another day, save it as text: see Saving a map and loading it later.
Read the map in Python
The map arrives as a nav_msgs/OccupancyGrid on /map, the same message real ROS robots use. It is a grid of small square cells, 5 cm × 5 cm each. Each cell holds one number:
| Value | Means |
|---|---|
| −1 | unknown: SLAM has not seen this cell yet |
| 0 to 49 | probably free floor (lower = more sure) |
| 50 to 100 | probably a wall or object (higher = more sure) |
The cells come as one long list, row after row, starting at the bottom-left corner of the map. NumPy turns it back into a grid with reshape(height, width), so grid[row, col] works. Rows go up along y, columns go along x.
# Read the SLAM map in Python · any world, SLAM on
# nav_msgs/OccupancyGrid: one number per 5 cm cell: -1 = unknown, otherwise 0..100 =
# how sure SLAM is that the cell is a wall. Treat 50 or more as a wall.
import numpy as np
import rclpy
from rclpy.node import Node
from nav_msgs.msg import OccupancyGrid
class MapReader(Node):
def __init__(self):
super().__init__('map_reader')
self.create_subscription(OccupancyGrid, '/map', self.on_map, 1)
def on_map(self, msg):
info = msg.info
grid = np.array(msg.data, dtype=np.int8).reshape(info.height, info.width) # grid[row, col]
unknown = (grid < 0).sum()
wall = (grid >= 50).sum()
free = ((grid >= 0) & (grid < 50)).sum()
self.get_logger().info(f'{info.width}x{info.height} cells of {info.resolution} m, '
f'origin ({info.origin.position.x:.2f}, {info.origin.position.y:.2f}) | '
f'free {free} wall {wall} unknown {unknown}')
# world (x, y) -> cell: col = (x - origin.x) / resolution, row = (y - origin.y) / resolution
x, y = 0.0, 0.0
col = int((x - info.origin.position.x) / info.resolution)
row = int((y - info.origin.position.y) / info.resolution)
self.get_logger().info(f'the cell at (0, 0) is {grid[row, col]}')
rclpy.init()
rclpy.spin(MapReader())From meters to cells and back
The map tells you two things you need: resolution (0.05 m per cell) and origin (where the bottom-left corner of the map is, in meters).
# meters -> cell
col = int((x - origin_x) / resolution)
row = int((y - origin_y) / resolution)
# cell -> meters (the middle of the cell)
x = origin_x + (col + 0.5) * resolution
y = origin_y + (row + 0.5) * resolutionWorked example, in the Maze. The origin is (−3.25, −3.25) and the resolution is 0.05. The robot starts at (−2, −2). Then col = (−2 − (−3.25)) ÷ 0.05 = 1.25 ÷ 0.05 = 25, and row = 25 as well. So the robot starts in cell grid[25, 25]. The middle of the world, (0, 0), is cell grid[65, 65].
How A* finds a path
A* on an occupancy grid: walls, explored cells, and the path it found
A* (say “A star”) is how most robots, and most video game characters, find their way. Picture the map as a chessboard. A* starts at the robot’s square and spreads out, always trying next the square that looks most promising. “Promising” means: steps taken so far + straight-line distance still to go. When it reaches the goal, it follows the trail back, and that is the shortest path.
- Grow the walls. The robot is not a dot; it is about 40 cm across. So we make every wall fatter by the robot’s radius (21 cm for the Scout). Now we can plan as if the robot were a point, and any path we find is wide enough for the real robot.
- Search. From the robot’s cell, A* looks at the 8 neighbors (straight steps cost 1, diagonal steps cost 1.414). It keeps a list of cells to try, sorted by cost so far + distance to the goal, and always takes the best one next.
- Unknown cells. Cells SLAM has not seen yet are allowed, but cost a bit more (×1.6). So the robot prefers known floor, but it is brave enough to explore toward a goal it cannot see yet.
- Path to waypoints. The path is hundreds of tiny cells. We keep every 4th one (every 20 cm) as a waypoint, and publish them on
/plan, so Singularity draws the path in the 3D view. - Drive (pure pursuit). Like following a friend on a bike: always steer toward a point a little way ahead on the path (30 cm). If that point is far to the side, turn on the spot first.
- Plan again when needed. Every new map is checked. If a newly seen wall now sits on our path, A* plans again.
- Bumps. Some things sit below the LiDAR beam (chair feet, table legs). If the bumper touches one, the robot backs away, marks that spot as an obstacle for 40 seconds, and plans around it.
Using it, step by step
- Pick a world. Maze is the best first try.
- Turn SLAM on (Map window → SLAM).
- Paste the program into a new file (☰ → New program) and press Run. The Terminal says Waiting for /map.
- In the Map window, press 2D Goal Pose, then click on the map where the robot should go (drag to set the direction).
- The path appears in the 3D view and the robot drives. The Terminal says Goal reached. when it arrives. Click a new goal any time.
| Setting at the top | What it does | Try |
|---|---|---|
ROBOT_RADIUS | How much the walls are grown. Too small: it scrapes corners. Too big: narrow gaps look closed. | 0.21 for the Scout, 0.28 for the Ranger |
LOOKAHEAD | How far ahead on the path it aims. Smaller follows the path more tightly; bigger is smoother. | 0.2 to 0.5 |
SPEED | Top driving speed in m/s. | 0.15 (careful) to 0.3 |
How well it works (our tests)
We ran the program in Singularity with SLAM on, clicking a goal at the far side of each world. Every run starts from the world’s start spot.
| World | Robot | Goal | Result |
|---|---|---|---|
| Maze | Scout and Ranger | (2, 2), the far corner | Reached in ~33 s, no bumps, ending within 10 cm |
| Playground | Scout and Ranger | (1.8, 1.8) | Reached in ~26 s, no bumps |
| Office | Scout | (3.2, −0.5), right side | Reached in ~20 s, no bumps |
| Office | Scout | (−3.1, −0.5), past the chairs | Reached every time (4 of 4), with a few bumps on chair feet the LiDAR cannot see |
| Warehouse | Scout | (3.2, −2.4), far corner | Reached in ~32 s, no bumps |
| Warehouse | Scout | (−3.2, 2.4), behind a rack | Sometimes: the gaps there are only a little wider than the robot plus its margin |
| Maze, saved map, SLAM off | Scout | (2, 2) | Reached, ending within 20 cm (odometry drifts a little) |
Saving a map and loading it later
Because there is no save button yet, we save the map the simple way: as text. Run this program with SLAM on and drive around for 20 seconds. It then prints the map in the Terminal, one character per 10 cm: # wall, . free, ? not seen.
# Save the SLAM map as text · any world, SLAM on
# Singularity 1.0 has no "save map" button, so this program prints the map in the
# Terminal as text. Copy it, keep it in a file, and paste it into SAVED_MAP in the
# A* program to use the same map later, even with SLAM switched off.
# # wall . free ? not seen yet (one character = 10 cm)
import numpy as np
import rclpy
from rclpy.node import Node
from nav_msgs.msg import OccupancyGrid
SECONDS = 20 # how long to wait for the map to fill in (drive around meanwhile!)
class MapSaver(Node):
def __init__(self):
super().__init__('map_saver')
self.map = None
self.create_subscription(OccupancyGrid, '/map', self.on_map, 1)
self.timer = self.create_timer(SECONDS, self.save)
self.get_logger().info(f'Collecting the map for {SECONDS} s. Drive around with W A S D!')
def on_map(self, msg):
self.map = msg
def save(self):
if self.map is None:
self.get_logger().warn('No map yet. Is SLAM on?')
return
info = self.map.info
g = np.array(self.map.data, dtype=np.int8).reshape(info.height, info.width)
g = g[: info.height // 2 * 2, : info.width // 2 * 2] # make the size even
blocks = g.reshape(g.shape[0] // 2, 2, g.shape[1] // 2, 2).swapaxes(1, 2).reshape(g.shape[0] // 2, g.shape[1] // 2, 4)
wall = (blocks >= 50).any(axis=2) # 2 x 2 cells -> one 10 cm cell
free = ((blocks >= 0) & (blocks < 50)).any(axis=2)
print(f"origin {info.origin.position.x:.2f} {info.origin.position.y:.2f} resolution {info.resolution * 2:.2f}")
for row in range(wall.shape[0] - 1, -1, -1): # top row first
print(''.join('#' if wall[row, c] else '.' if free[row, c] else '?' for c in range(wall.shape[1])))
print('--- end of map: copy everything from the "origin" line to here ---')
self.timer.cancel() # print it once; press Stop when you have copied it
rclpy.init()
rclpy.spin(MapSaver())- Select the printed lines in the Terminal, from the
origin …line down to the end line, and copy them (Ctrl + C). - Keep them in a text file, or in a new Singularity file, so they are safe.
- To use the map: in the navigation program, paste them between the triple quotes of
SAVED_MAP = """ … """. - Now you can run the navigator with SLAM off. It loads the saved map and finds its position from wheel odometry (
/odom) instead.
Drawing your own map
You can also make a map by hand, as text, and publish it on /map. It appears in the Map window. This is a nice way to see how an occupancy grid is put together, and to test a planner on a map you control.
# Make your own map and show it · any world (SLAM off)
# Draw the map as text, turn it into an OccupancyGrid, publish it on /map.
# It appears in the Map window, and your planner can use it like a SLAM map.
import rclpy
from rclpy.node import Node
from nav_msgs.msg import OccupancyGrid
DRAWING = """
##########
#........#
#..####..#
#..#.....#
#..#..####
#........#
##########
"""
CELL = 0.5 # meters per character
class MapMaker(Node):
def __init__(self):
super().__init__('map_maker')
rows = [r for r in DRAWING.strip().splitlines()][::-1] # first text row = top of the map
msg = OccupancyGrid()
msg.header.frame_id = 'map'
msg.info.resolution = CELL
msg.info.width, msg.info.height = len(rows[0]), len(rows)
msg.info.origin.position.x = -msg.info.width * CELL / 2
msg.info.origin.position.y = -msg.info.height * CELL / 2
msg.data = [100 if ch == '#' else 0 if ch == '.' else -1 for row in rows for ch in row]
self.create_publisher(OccupancyGrid, '/map', 1).publish(msg)
self.get_logger().info(f'Published a {msg.info.width}x{msg.info.height} map.')
rclpy.init()
rclpy.spin(MapMaker())Make it better: ideas to try
- Stay in the middle of corridors. Make cells near walls cost more (a “distance cost”). The robot will keep away from walls instead of brushing past them. Real planners (Nav2) do this with a costmap.
- Smoother paths. After A*, remove waypoints that lie on a straight line, or join waypoints that can “see” each other without crossing a wall.
- Explore by itself. Find the free cells next to unknown cells (frontiers), and send the robot to the nearest one. Repeat until there are none left: the robot maps the whole world on its own.
- Compare. Try Dijkstra (A* with the distance-to-go part set to 0). It finds the same path but looks at many more cells. Count them.
Stuck on something this page did not answer? Write to us at antiprotontechnologies@gmail.com. Paste what you tried and what happened (the Copy details for support button in Singularity’s Help & Support window helps us a lot).