Capstone: a small mobile robot program

This lesson brings the whole course together in one project: a safe teleoperation system for a small mobile robot. A person drives the robot from a keyboard. A safety filter node sits between the keyboard and the wheels: it passes commands through, but refuses to drive forward into an obstacle detected by the laser scanner, and stops the robot entirely if either the commands or the scans stop arriving. The project uses almost everything you have learned: Linux and Git for the workspace, Python classes and timing, publishers and subscribers, quality of service, parameters, a package with an entry point, a launch file, transforms in the background, simulation and recorded data. It runs unchanged on a simulated robot in Gazebo or on a real one such as a TurtleBot.

Requirements

Professional robot software starts with written requirements, which say what the system must do and how you will know it does it. Good requirements are specific and testable: the robot shall not drive forward when an obstacle is within the stopping distance is testable; the robot shall be safe is not. For this project:

• R1. The operator drives the robot with the keyboard; commands reach the base within 100 ms.
• R2. When any obstacle is within the stop distance in a ±20° cone ahead, forward motion is set to zero, while turning and reversing remain possible.
• R3. If no command or no scan has arrived for longer than the timeout, the robot is commanded to stop.
• R4. Velocity commands are published at a steady 20 Hz so the base driver's own timeout can act if the safety filter fails.
• R5. The stop distance, timeout and rate are parameters, validated and documented.
• R6. The node publishes its state (DRIVING, BLOCKED or STALE) on a status topic.
• R7. One launch file starts the system on the real robot or in simulation, optionally recording a bag.
• R8. The decision logic has automated tests that run with colcon test.

System design

The design follows the patterns from earlier lessons. The keyboard node publishes on teleop_cmd. The safety_filter node subscribes to teleop_cmd and to scan, and publishes on cmd_vel, where the base driver or the simulator listens, and on its private status topic. All the decision logic lives in a plain Python class, SafetyFilter, in a module called logic.py, together with the front_min function from lesson 6. The ROS node is a thin layer that converts messages into calls on that class, so the logic can be tested without ROS.

The filter has three states. In DRIVING it passes commands through unchanged. In BLOCKED, entered when an obstacle is inside the stop distance and the operator asks to go forward, it zeroes the forward speed but passes turning and reversing. In STALE, entered whenever the latest command or scan is older than the timeout, it commands a full stop. Thinking in explicit states makes the behaviour easy to describe, test and display, and is the simplest form of the state machines you met in ROB-100.

The logic, tested first

Here is the core class, followed by a scenario that feeds it a sequence of scans and commands, much as a recorded bag would. Read the output against the requirements: at 1.0 s an obstacle 0.35 m ahead blocks forward motion but the turn passes through (R2); at 1.5 s reversing is allowed; and at 2.5 s, with no new messages for a second, the filter reports STALE and commands a stop (R3).

class SafetyFilter:
    """Decides the command to send; knows nothing about ROS."""

    def __init__(self, stop_distance=0.4, timeout=0.5):
        self.stop_distance = stop_distance
        self.timeout = timeout
        self.cmd, self.cmd_time = (0.0, 0.0), None
        self.front, self.scan_time = float("inf"), None

    def on_scan(self, t, front_distance):
        self.front, self.scan_time = front_distance, t

    def on_cmd(self, t, v, w):
        self.cmd, self.cmd_time = (v, w), t

    def step(self, t):
        """Return (v, w, state) to publish at time t."""
        for stamp in (self.cmd_time, self.scan_time):
            if stamp is None or t - stamp > self.timeout:
                return 0.0, 0.0, "STALE"
        v, w = self.cmd
        if v > 0.0 and self.front < self.stop_distance:
            return 0.0, w, "BLOCKED"
        return v, w, "DRIVING"


f = SafetyFilter()
events = [  # (time, kind, values)
    (0.0, "scan", 2.0), (0.0, "cmd", (0.3, 0.0)),
    (0.5, "scan", 0.9), (0.5, "cmd", (0.3, 0.0)),
    (1.0, "scan", 0.35), (1.0, "cmd", (0.3, 0.5)),
    (1.5, "scan", 0.35), (1.5, "cmd", (-0.1, 0.0)),
]
for t, kind, val in events:
    if kind == "scan":
        f.on_scan(t, val)
    else:
        f.on_cmd(t, *val)
        print(f"t={t:.1f} s ->", f.step(t))
print("t=2.5 s ->", f.step(2.5), " (no new messages)")

t=0.0 s -> (0.3, 0.0, 'DRIVING')
t=0.5 s -> (0.3, 0.0, 'DRIVING')
t=1.0 s -> (0.0, 0.5, 'BLOCKED')
t=1.5 s -> (-0.1, 0.0, 'DRIVING')
t=2.5 s -> (0.0, 0.0, 'STALE')  (no new messages)

Automated tests turn scenarios like this into checks that run every time the code changes. ROS 2's Python packages use pytest; a test is any function whose name starts with test_ in a file in the package's test folder, and colcon test runs them all along with the style checks that ros2 pkg create set up. Each test sets up a situation, runs the logic and asserts the expected result.

# test/test_logic.py, run with colcon test or pytest
from my_robot.logic import SafetyFilter


def test_blocks_forward_but_allows_turning():
    f = SafetyFilter(stop_distance=0.4, timeout=0.5)
    f.on_scan(0.0, 0.3)
    f.on_cmd(0.0, 0.2, 0.5)
    assert f.step(0.1) == (0.0, 0.5, 'BLOCKED')


def test_stops_when_commands_go_stale():
    f = SafetyFilter(timeout=0.5)
    f.on_scan(0.0, 2.0)
    f.on_cmd(0.0, 0.2, 0.0)
    f.on_scan(1.0, 2.0)
    assert f.step(1.0)[2] == 'STALE'

Choosing the numbers

A safety filter is only as good as its parameters. The stop distance must be larger than the distance the robot needs to stop after an obstacle appears, which has two parts: the distance travelled while the system reacts, and the braking distance. The reaction time includes up to one scan period before the obstacle is seen, the processing time and up to one command period before the new command is sent, plus the base's own response.

The node, the package and the launch file

With the logic tested, the node is wiring. It declares the three parameters, creates the subscriptions and publishers, and runs a 20 Hz timer that asks the filter what to publish, so commands go out at a steady rate whether or not the operator is pressing keys (R4). It timestamps every event with the node's clock, so it works in simulated time too. Parameter validation from lesson 7 can be added in the same way as before.

import rclpy
from rclpy.node import Node
from rclpy.executors import ExternalShutdownException
from rclpy.qos import qos_profile_sensor_data
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
from std_msgs.msg import String
from my_robot.logic import SafetyFilter, front_min


class SafetyNode(Node):
    def __init__(self):
        super().__init__('safety_filter')
        self.declare_parameter('stop_distance', 0.4)
        self.declare_parameter('timeout', 0.5)
        self.declare_parameter('rate', 20.0)
        p = lambda n: self.get_parameter(n).value
        self.filter = SafetyFilter(p('stop_distance'), p('timeout'))
        self.create_subscription(LaserScan, 'scan', self.on_scan,
                                 qos_profile_sensor_data)
        self.create_subscription(Twist, 'teleop_cmd', self.on_cmd, 10)
        self.cmd_pub = self.create_publisher(Twist, 'cmd_vel', 10)
        self.status_pub = self.create_publisher(String, '~/status', 10)
        self.create_timer(1.0 / p('rate'), self.on_timer)

    def now(self):
        return self.get_clock().now().nanoseconds * 1e-9

    def on_scan(self, scan):
        d = front_min(scan.ranges, scan.angle_min, scan.angle_increment)
        self.filter.on_scan(self.now(), d)

    def on_cmd(self, cmd):
        self.filter.on_cmd(self.now(), cmd.linear.x, cmd.angular.z)

    def on_timer(self):
        v, w, state = self.filter.step(self.now())
        out = Twist()
        out.linear.x, out.angular.z = v, w
        self.cmd_pub.publish(out)
        self.status_pub.publish(String(data=state))


def main(args=None):
    rclpy.init(args=args)
    node = SafetyNode()
    try:
        rclpy.spin(node)
    except (KeyboardInterrupt, ExternalShutdownException):
        pass
    finally:
        node.destroy_node()
        rclpy.try_shutdown()

Add an entry point, safety_filter = my_robot.safety_node:main, to setup.py, and depend on rclpy, geometry_msgs, sensor_msgs and std_msgs in package.xml. The launch file starts the filter and the keyboard node, passes use_sim_time through so the same file serves simulation and hardware, and optionally starts a bag recording of every topic the filter uses (R7).

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    sim = LaunchConfiguration('use_sim_time')
    record = LaunchConfiguration('record')
    return LaunchDescription([
        DeclareLaunchArgument('use_sim_time', default_value='false'),
        DeclareLaunchArgument('record', default_value='false'),
        Node(package='my_robot', executable='safety_filter',
             parameters=[{'use_sim_time': sim, 'stop_distance': 0.5}]),
        Node(package='teleop_twist_keyboard',
             executable='teleop_twist_keyboard', prefix='xterm -e',
             parameters=[{'use_sim_time': sim}],
             remappings=[('cmd_vel', 'teleop_cmd')]),
        ExecuteProcess(
            cmd=['ros2', 'bag', 'record', '/scan', '/odom', '/tf',
                 '/tf_static', '/teleop_cmd', '/cmd_vel',
                 '/safety_filter/status'],
            condition=IfCondition(record)),
    ])

Testing the whole system

Test in stages, from cheapest to most realistic, and record what you did. First, colcon test runs the unit tests. Second, replay a bag recorded from a real robot through the filter and check its status output with ros2 topic echo. Third, run in Gazebo with use_sim_time:=true, drive towards a wall and check that the robot stops short of it, that it can turn away, and that stopping the keyboard node makes it halt within the timeout. Finally, test on the real robot in a clear area at low speed, with someone ready at the emergency stop, recording a bag throughout.

Keep the test results with the code: the commit you tested, recorded with git describe, the bags you recorded and any failures and fixes. A short write-up with the requirements, design diagram, test table and results is exactly the kind of portfolio piece that shows an employer or graduate supervisor you can build robot software properly, not just make a demonstration work once.

Extending the project

There are many ways to take the project further, each preparing for later courses. Scale the forward speed down smoothly as obstacles approach instead of stopping abruptly, which needs a little control theory from ROB-300. Watch behind the robot when reversing. Use the transform from laser to base_link, as in lesson 10, so the filter works whatever way the scanner is mounted. Publish a visualization marker in RViz showing the stop cone. Replace the keyboard with a joystick, or with a Nav2 navigation stack, and keep the filter as an independent safety layer. And port the node to C++ with rclcpp to compare performance.

Robotics software skills like these are in demand across Canada, in robotics companies, manufacturers, agriculture and mining technology firms, and research institutes. Waterloo, Toronto, Montreal, Edmonton and Vancouver all have active robotics and AI communities and university labs, and many companies hire students for co-op terms, so a well-documented ROS 2 project on GitHub is a strong start to an application. Employment and Social Development Canada's Job Bank publishes outlooks for related occupations such as software developers and electrical and mechanical engineering technologists by province.