Python for robot programs

Python is the language most people use to write their first robot programs, and many professional robots use it for high-level behaviour, testing and tools. It is quick to write, easy to read, and has excellent libraries for numerical work, vision and machine learning. ROS 2 supports it through a client library called rclpy, alongside rclcpp for C++. This lesson builds on the Python you learned in CMPUT-174 and focuses on the habits that matter when code controls a physical machine: organizing state in classes, working with arrays of sensor data in NumPy, keeping time correctly in loops, and making sure that errors stop a robot safely rather than leaving it running blind.

Python or C++?

Robot software is a mix of languages. Low-level drivers and fast control loops are usually written in C or C++ because they compile to efficient machine code with predictable timing. Python is interpreted, slower and has occasional pauses for memory management, so it is a poor choice for a loop that must run every millisecond. But a great deal of robot software runs at 10 to 100 Hz, spends most of its time waiting for messages, or calls libraries such as NumPy and OpenCV that are themselves written in C. For that code Python's speed of development wins. In ROS 2, Python and C++ nodes communicate seamlessly, so teams commonly prototype in Python and rewrite only the parts that profiling shows to be too slow.

Organizing robot state with classes

A robot program has to remember things: its estimated position, the latest sensor readings, the current goal, the settings it was started with. Scattering these across global variables quickly becomes unmanageable. Classes group related data and the functions that act on it, and ROS 2 nodes in Python are themselves classes, as you will see in lesson 6. A dataclass is a compact way to define a class that mainly holds data: Python writes the constructor and a readable printed form for you.

The listing below models a differential-drive robot, the two-wheeled base you met in ROB-100. A Pose2D dataclass holds the position and heading, and a DiffDriveModel class updates the pose from a forward speed v and a turning rate ω over a short time step, using the same geometry as wheel odometry. The program drives the model forward, turns it left on the spot and drives it forward again.

import math
from dataclasses import dataclass


@dataclass
class Pose2D:
    x: float = 0.0
    y: float = 0.0
    theta: float = 0.0  # heading in radians


class DiffDriveModel:
    """Tracks the pose of a differential-drive robot."""

    def __init__(self, wheel_base):
        self.wheel_base = wheel_base
        self.pose = Pose2D()

    def update(self, v, w, dt):
        """Advance the pose for dt seconds at speed v and turn rate w."""
        p = self.pose
        p.x += v * math.cos(p.theta) * dt
        p.y += v * math.sin(p.theta) * dt
        p.theta += w * dt


robot = DiffDriveModel(wheel_base=0.3)
for step in range(20):          # 2 s straight at 0.5 m/s
    robot.update(0.5, 0.0, 0.1)
for step in range(10):          # 1 s turning left at 90 deg/s
    robot.update(0.0, math.pi / 2, 0.1)
for step in range(20):          # 2 s straight again
    robot.update(0.5, 0.0, 0.1)
p = robot.pose
print(f"x = {p.x:.2f} m, y = {p.y:.2f} m, "
      f"heading = {math.degrees(p.theta):.0f} deg")

x = 1.00 m, y = 1.00 m, heading = 90 deg

The robot ends one metre forward and one metre to the left, facing 90 degrees, as you would expect from the commands. Small design choices make this code easier to reuse: the wheel base is passed to the constructor rather than hard-coded, the time step is an argument so the same class works at any loop rate, and the angle is kept in radians, the unit every maths library expects. Type hints, docstrings and names that say what a value means, such as wheel_base rather than wb, cost little and save a great deal of confusion when a teammate reads your node at midnight before a competition.

The update uses simple Euler integration: it assumes the heading stays constant during each step. That is accurate when the step is small compared with how fast the robot turns. Halving the time step roughly halves the error, so a model updated at 50 Hz tracks a turning robot better than one updated at 5 Hz. You will meet better integration methods and the full odometry equations in ROB-210, but the habit of asking how a time step affects accuracy starts here.

Working with sensor data in NumPy

Sensors produce arrays of numbers: a laser scanner returns hundreds of distances per scan, a depth camera hundreds of thousands per frame. Processing them with Python loops is slow; NumPy processes whole arrays at once in compiled code, often a hundred times faster, and the code is shorter. The key ideas are that arithmetic on arrays applies element by element, that comparisons produce arrays of true and false values called masks, and that a mask can select elements from another array.

A two-dimensional laser scan reports each beam's range r at a known angle θ. Converting to Cartesian coordinates in the sensor's frame uses the familiar polar-to-Cartesian formulas, applied to all beams at once:

Scanners report beams that hit nothing as infinity or as a value outside their valid range, so real code removes them first. The listing builds a small simulated scan, removes the invalid beam with a mask, converts the rest to points and finds the nearest obstacle.

import numpy as np

# a simulated laser scan: 9 beams from -60 to +60 degrees
angles = np.radians(np.linspace(-60, 60, 9))
ranges = np.array([2.0, 1.8, 1.5, 0.7, 0.9, 0.8, 1.6, np.inf, 2.5])

valid = np.isfinite(ranges)          # drop beams with no return
x = ranges[valid] * np.cos(angles[valid])
y = ranges[valid] * np.sin(angles[valid])

j = np.argmin(ranges[valid])         # nearest valid beam
print("points:", valid.sum())
print(f"nearest: {ranges[valid][j]:.2f} m at "
      f"{np.degrees(angles[valid][j]):+.0f} deg")
print(f"as a point: x = {x[j]:.2f} m, y = {y[j]:.2f} m")

points: 8
nearest: 0.70 m at -15 deg
as a point: x = 0.68 m, y = -0.18 m

Note that NumPy's argmin was applied to the filtered array, not the original, so the index j matches the x and y arrays. Keeping track of which array an index refers to after filtering is one of the most common sources of subtle bugs in sensor code. Another is mixing degrees and radians: NumPy's trigonometric functions expect radians, so convert once, at the edge of your program, and work in radians everywhere else.

Keeping time in loops

Robot programs run in loops: read sensors, decide, command actuators, wait, repeat. The loop must run at a steady rate, because controllers and estimators assume a known time step. Two time functions matter. time.time() returns the wall-clock time, which can jump when the clock is synchronized or changed; time.monotonic() never goes backwards and is the right choice for measuring intervals. In ROS 2 you will normally let the framework call your code from a timer, but understanding what goes wrong with hand-written loops explains why.

The naive approach is to do the work and then call time.sleep for the full period. But the work itself takes time, so each iteration lasts the work time plus the sleep, and the loop runs slower than intended. The fix is to schedule against deadlines: keep track of when the next iteration should start and sleep only until then. The listing simulates both approaches for a 50 Hz loop whose work takes 12 milliseconds, without actually sleeping, so the result is exact.

# Simulated timing: each loop does 12 ms of work, then waits.
period = 0.020                 # target 50 Hz
work = 0.012

# naive: work, then sleep a full period every time
t = 0.0
for k in range(500):
    t += work + period
print(f"naive: 500 loops took {t:.2f} s, "
      f"rate {500 / t:.1f} Hz")

# deadline-based: sleep only until the next tick
t, next_tick = 0.0, 0.0
for k in range(500):
    t += work
    next_tick += period
    t = max(t, next_tick)      # sleep until the deadline
print(f"deadline: 500 loops took {t:.2f} s, "
      f"rate {500 / t:.1f} Hz, busy {work / period:.0%}")

naive: 500 loops took 16.00 s, rate 31.2 Hz
deadline: 500 loops took 10.00 s, rate 50.0 Hz, busy 60%

The naive loop runs at about 31 Hz instead of 50 Hz, a 38 per cent error that would make a controller tuned for 50 Hz behave quite differently. The deadline-based loop holds 50 Hz and spends 60 per cent of each period working. That fraction, the utilization, is worth computing for every loop: if the work ever takes longer than the period, the loop cannot keep up, and you must make the work faster, lower the rate or move it to another thread or process. Real loops also show jitter, small random variations in when each iteration starts, caused by the operating system scheduling other processes; lesson 6 shows how to measure it in ROS 2.

Failing safely

When a program that controls a robot crashes, the robot does not politely stop. A motor controller that received a velocity command will usually keep executing it until it receives another command or until its own watchdog timer expires. Software therefore needs to handle errors deliberately. Python's try and finally statements guarantee that cleanup code runs whether the protected block finishes normally, raises an exception or is interrupted by Ctrl+C. Put the code that stops actuators in a finally block, or in the shutdown handler that ROS 2 provides, so that every exit path leaves the robot in a safe state.

class FakeMotor:
    def __init__(self):
        self.speed = 0.0

    def set_speed(self, s):
        self.speed = s
        print(f"motor speed set to {s}")


def drive(motor, readings):
    try:
        for r in readings:
            if r < 0:
                raise ValueError(f"bad sensor reading {r}")
            motor.set_speed(min(r, 1.0))
    finally:
        motor.set_speed(0.0)   # always runs, even after an error


m = FakeMotor()
try:
    drive(m, [0.4, 0.8, -1.0, 0.6])
except ValueError as err:
    print("stopped because:", err)
print("final speed:", m.speed)

motor speed set to 0.4
motor speed set to 0.8
motor speed set to 0.0
stopped because: bad sensor reading -1.0
final speed: 0.0

The third sensor reading is invalid, so drive raises an exception, but the finally block still sets the speed to zero before the error reaches the caller. Software stops are not the only safety layer: real robots also have hardware emergency stops, controller watchdogs that stop the motors if commands cease, and limits enforced in firmware. ROB-450 covers these in depth. But good software should never be the reason a robot keeps moving after something has gone wrong.

Writing code others can maintain

Robot software is usually written by teams and lives for years. A few habits pay off quickly. Follow the PEP 8 style guide so that your code looks like everyone else's; tools such as flake8 or ruff check it automatically, and ROS 2's own build tests can run them. Use the logging facilities rather than print, so that messages have severity levels and timestamps and can be filtered; in ROS 2 that means the node's logger. Keep hardware access, algorithms and ROS plumbing in separate functions or classes so that you can test the algorithm with fake data on a laptop, as the DiffDriveModel above can be tested without a robot. And write small automated tests with pytest for anything with arithmetic in it, because a sign error in a heading calculation is much cheaper to find in a test than in a robot driving into a wall.