Line-follower → Navigator.

A robot that follows a line, then thinks bigger

This worked example builds a single ROS package, line_navigator, on a two-wheeled differential-drive robot (the kind simulated on the Robotnik SUMMIT-XL in the navigation sessions). The robot starts with one job — stay centered on a colored line painted on the floor — and we show how the exact same node graph generalizes to goal-directed autonomous navigation. It is deliberately the smallest project that touches every stage of the sense → plan → act loop the course is built around.

What it exercises

• ROS plumbing — nodes, topics, message types, publish/subscribe, launch files.
• Perception — an OpenCV HSV color threshold turning a camera image into a single line-position error.
• Kinematics — the differential-drive model converting (v, ω) into wheel commands and integrating odometry.
• Control — a discrete PID loop on the heading error, with anti-windup and output clamping.
• Planning / behavior — a finite state machine (DRIVE · SEARCH · STOP) that reacts to perception confidence.

Stack & assumptions

The code is written twice where it matters: the canonical version uses rospy (ROS 1), and the perception node also shows the rclpy (ROS 2) equivalent so the project reads cleanly on either distribution. The math and the control logic are identical across both.

The ROS node graph

A running ROS system is a graph of independent processes (nodes) that pass typed messages over named topics. This project uses three nodes of our own plus the simulator/driver, wired in a clean perception → control → actuation chain. The diagram below is exactly what rqt_graph would draw.

      [ camera driver ]                                   [ diff-drive base ]
       /camera/image_raw                                   wheel motors
              |  sensor_msgs/Image                                ^
              v                                                   | geometry_msgs/Twist
   +---------------------+      std_msgs/Float32      +---------------------+
   |  perception_node    | -----------------------> |   controller_node   |
   |  HSV threshold +    |   /line/error  (px)        |  PID(heading) +     |
   |  centroid           | -----------------------> |  diff-drive mux      |
   |                     |   /line/detected (Bool)    |                     |
   +---------------------+                            +----------+----------+
                                                                 | geometry_msgs/Twist
                                                                 v   /cmd_vel
                                                       +---------------------+
                                                       |   behavior_node     |
                                                       |  FSM: DRIVE /        |
                                                       |  SEARCH / STOP       |
                                                       +---------------------+
                                                          ^            |
                                /line/detected (Bool) ----+            +---> /robot/state (String)
                                /odom  (nav_msgs/Odometry) ----------------> (debug / RViz)

Topics & message types

The math under the wheels

Differential-drive kinematics

A differential-drive robot has two independently driven wheels of radius r a distance L apart (the wheel base). Given wheel angular speeds ωL and ωR, the wheel ground speeds are vL = r·ωL and vR = r·ωR. The robot's body twist is:

For control we want the inverse: given the commanded (v, ω) from the controller, solve back for the wheel speeds the base needs:

Integrating the body twist over a timestep Δt gives the odometry update — the pose (x, y, θ) the robot believes it is at (exact for the small-angle / midpoint form):

The PID heading controller

The line-position error e (pixels off-center) is what we want to drive to zero. A PID controller produces an angular-velocity command ω from that error:

In code we use the discrete (sampled) form at fixed timestep Δt:

Building the three nodes

Below is the full, runnable code for the package. It is correct ROS Python: every subscriber, publisher, message type, and spin loop is real. Read it top to bottom — perception first (turn pixels into an error), then control (turn the error into a twist), then behavior (decide when to drive).

The perception node subscribes to the camera, converts the bottom strip of the frame to HSV (robust to lighting, exactly the logic the vision sessions teach), masks the line color, and computes the mask's centroid with image moments. The horizontal offset of that centroid from the image center is the error we publish.

#!/usr/bin/env python3
# Perception: camera image -> horizontal line error (pixels) + detected flag.
import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image
from std_msgs.msg import Float32, Bool
from cv_bridge import CvBridge


class PerceptionNode:
    def __init__(self):
        self.bridge = CvBridge()
        # HSV bounds for the line color (yellow line on a dark floor).
        # Tune these in the #vision demo; H in [0,180] for OpenCV.
        self.lower = np.array([20, 100, 100], dtype=np.uint8)
        self.upper = np.array([35, 255, 255], dtype=np.uint8)
        self.min_area = rospy.get_param("~min_area", 500)

        self.err_pub = rospy.Publisher("/line/error", Float32, queue_size=1)
        self.det_pub = rospy.Publisher("/line/detected", Bool, queue_size=1)
        self.sub = rospy.Subscriber("/camera/image_raw", Image,
                                    self.on_image, queue_size=1)
        rospy.loginfo("perception_node ready")

    def on_image(self, msg):
        frame = self.bridge.imgmsg_to_cv2(msg, "bgr8")
        h, w = frame.shape[:2]

        # Only look at the bottom third: that is the road right in front.
        roi = frame[int(2 * h / 3):h, 0:w]
        hsv = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV)
        mask = cv2.inRange(hsv, self.lower, self.upper)
        mask = cv2.erode(mask, None, iterations=2)
        mask = cv2.dilate(mask, None, iterations=2)

        M = cv2.moments(mask)
        if M["m00"] > self.min_area:
            cx = M["m10"] / M["m00"]      # centroid x in ROI pixels
            error = float(cx - w / 2.0)        # +ve => line is right of center
            self.err_pub.publish(Float32(error))
            self.det_pub.publish(Bool(True))
        else:
            # No line this frame: report not-detected, hold last error untouched.
            self.det_pub.publish(Bool(False))


if __name__ == "__main__":
    rospy.init_node("perception_node")
    PerceptionNode()
    rospy.spin()

For ROS 2, the structure is identical; only the API names change:

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
import cv2, numpy as np
from sensor_msgs.msg import Image
from std_msgs.msg import Float32, Bool
from cv_bridge import CvBridge


class PerceptionNode(Node):
    def __init__(self):
        super().__init__("perception_node")
        self.bridge = CvBridge()
        self.lower = np.array([20, 100, 100], np.uint8)
        self.upper = np.array([35, 255, 255], np.uint8)
        self.err_pub = self.create_publisher(Float32, "/line/error", 1)
        self.det_pub = self.create_publisher(Bool, "/line/detected", 1)
        self.create_subscription(Image, "/camera/image_raw", self.on_image, 1)

    def on_image(self, msg):
        frame = self.bridge.imgmsg_to_cv2(msg, "bgr8")
        h, w = frame.shape[:2]
        hsv = cv2.cvtColor(frame[int(2 * h / 3):h], cv2.COLOR_BGR2HSV)
        mask = cv2.inRange(hsv, self.lower, self.upper)
        M = cv2.moments(mask)
        if M["m00"] > 500:
            cx = M["m10"] / M["m00"]
            self.err_pub.publish(Float32(data=float(cx - w / 2.0)))
            self.det_pub.publish(Bool(data=True))
        else:
            self.det_pub.publish(Bool(data=False))


def main():
    rclpy.init()
    rclpy.spin(PerceptionNode())
    rclpy.shutdown()

The controller holds a small, self-contained PID class (anti-windup + output clamp), runs it at a fixed 20 Hz on the latest line error, and converts the result into a geometry_msgs/Twist. It also implements the inverse differential-drive equations so the same node could publish raw wheel speeds if the base wanted them.

#!/usr/bin/env python3
# Control: line error -> PID -> Twist on /cmd_vel.
import rospy
from std_msgs.msg import Float32, Bool
from geometry_msgs.msg import Twist


class PID:
    def __init__(self, kp, ki, kd, out_limit, i_limit):
        self.kp, self.ki, self.kd = kp, ki, kd
        self.out_limit = out_limit       # clamp on |u|
        self.i_limit = i_limit           # anti-windup clamp on integral
        self.integral = 0.0
        self.prev_e = 0.0

    def reset(self):
        self.integral = 0.0
        self.prev_e = 0.0

    def step(self, e, dt):
        if dt <= 0.0:
            return 0.0
        self.integral += e * dt
        # Anti-windup: keep the integral term bounded.
        self.integral = max(-self.i_limit, min(self.i_limit, self.integral))
        derivative = (e - self.prev_e) / dt
        self.prev_e = e
        u = self.kp * e + self.ki * self.integral + self.kd * derivative
        return max(-self.out_limit, min(self.out_limit, u))


class ControllerNode:
    def __init__(self):
        # Gains are in rad/s per pixel; tuned in the results table below.
        self.pid = PID(kp=0.004, ki=0.0002, kd=0.0015,
                       out_limit=2.0, i_limit=500.0)
        self.base_speed = rospy.get_param("~base_speed", 0.22)   # m/s
        self.wheel_base = rospy.get_param("~wheel_base", 0.30)   # L, meters
        self.error = 0.0
        self.detected = False

        self.cmd_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
        rospy.Subscriber("/line/error", Float32, self.on_error, queue_size=1)
        rospy.Subscriber("/line/detected", Bool, self.on_detected, queue_size=1)

        self.last_t = rospy.Time.now().to_sec()
        self.timer = rospy.Timer(rospy.Duration(1.0 / 20.0), self.update)  # 20 Hz

    def on_error(self, msg):
        self.error = msg.data

    def on_detected(self, msg):
        if not msg.data:
            self.pid.reset()            # drop integral when the line is lost
        self.detected = msg.data

    def wheel_speeds(self, v, w):
        # Inverse differential-drive kinematics (m/s at each wheel).
        vl = v - (w * self.wheel_base) / 2.0
        vr = v + (w * self.wheel_base) / 2.0
        return vl, vr

    def update(self, _event):
        now = rospy.Time.now().to_sec()
        dt = now - self.last_t
        self.last_t = now

        twist = Twist()
        if self.detected:
            u = self.pid.step(self.error, dt)
            twist.angular.z = -u                 # +error (line right) => turn right
            # Slow down proportionally to how hard we are turning.
            twist.linear.x = self.base_speed * (1.0 - min(0.6, abs(u) / 2.0))
        # If not detected, leave twist zero; behavior_node overrides for SEARCH.
        self.cmd_pub.publish(twist)


if __name__ == "__main__":
    rospy.init_node("controller_node")
    ControllerNode()
    rospy.spin()

The behavior node is the "plan" stage. It watches /line/detected and sequences three states: DRIVE (let the controller follow the line), SEARCH (line lost — rotate in place to re-acquire it, with a timeout), and STOP (search failed — halt safely). It publishes its own /cmd_vel only when overriding the controller, and announces the current state on /robot/state.

#!/usr/bin/env python3
# Behavior: finite state machine over line-detection confidence.
import rospy
from std_msgs.msg import Bool, String
from geometry_msgs.msg import Twist

DRIVE, SEARCH, STOP = "DRIVE", "SEARCH", "STOP"


class BehaviorNode:
    def __init__(self):
        self.state = DRIVE
        self.detected = True
        self.lost_since = None
        self.search_timeout = rospy.get_param("~search_timeout", 4.0)  # s
        self.search_omega = rospy.get_param("~search_omega", 0.6)    # rad/s

        self.cmd_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
        self.state_pub = rospy.Publisher("/robot/state", String, queue_size=1)
        rospy.Subscriber("/line/detected", Bool, self.on_detected, queue_size=1)
        self.timer = rospy.Timer(rospy.Duration(1.0 / 20.0), self.update)

    def on_detected(self, msg):
        self.detected = msg.data

    def transition(self):
        now = rospy.Time.now().to_sec()
        if self.detected:
            self.lost_since = None
            self.state = DRIVE
            return
        # Line not detected: start/continue a search, give up after timeout.
        if self.lost_since is None:
            self.lost_since = now
        elapsed = now - self.lost_since
        self.state = SEARCH if elapsed < self.search_timeout else STOP

    def update(self, _event):
        self.transition()
        self.state_pub.publish(String(self.state))

        # Only the behavior node drives in SEARCH / STOP; DRIVE defers to controller.
        if self.state == SEARCH:
            twist = Twist()
            twist.angular.z = self.search_omega   # spin in place to re-find the line
            self.cmd_pub.publish(twist)
        elif self.state == STOP:
            self.cmd_pub.publish(Twist())          # all-zero == halt


if __name__ == "__main__":
    rospy.init_node("behavior_node")
    BehaviorNode()
    rospy.spin()

A single launch file starts all three nodes (plus the simulator) and sets the tunable parameters in one place. This is what you actually run: roslaunch line_navigator line_follow.launch.

<launch>
  <node name="perception_node" pkg="line_navigator"
        type="perception_node.py" output="screen">
    <param name="min_area" value="500" />
  </node>

  <node name="controller_node" pkg="line_navigator"
        type="controller_node.py" output="screen">
    <param name="base_speed" value="0.22" />
    <param name="wheel_base" value="0.30" />
  </node>

  <node name="behavior_node" pkg="line_navigator"
        type="behavior_node.py" output="screen">
    <param name="search_timeout" value="4.0" />
    <param name="search_omega" value="0.6" />
  </node>
</launch>

What it does — and how it was tuned

With the launch file running against a yellow oval track in Gazebo, the robot acquires the line immediately, holds the centroid within a few pixels of center on straights, and tracks gentle curves smoothly. On a sharp hairpin the line briefly leaves the bottom-third ROI; the FSM flips to SEARCH, the robot pivots, re-acquires within well under a second, and returns to DRIVE. Pulling the track tape away entirely sends it through SEARCH to a clean STOP after the 4-second timeout — no runaway.

Tuning the PID gains

Gains were found with the classic procedure: raise Kp until the robot tracks but oscillates, add Kd to damp the oscillation, then a touch of Ki to remove the steady-state bias on long curves. Units are rad/s of yaw per pixel of error.

Behavior trace

Where this project meets the syllabus

Every piece of this project lands on a specific course learning outcome. This is the project you could submit for the session-19 mid-term, which asks for an individual ROS program that makes a simulated robot sense, plan, and act.

• Build ROS systems from nodes communicating over topics. — three nodes, six topics, real publishers/subscribers and a launch file (sessions 3–4, ROS demo).
• Solve forward kinematics / reason about motion. — the differential-drive equations and odometry integration in §3 (sessions 7–8, drive demo).
• Design and tune PID control loops. — a from-scratch PID with anti-windup, tuned in the §5 table (sessions 9–10, control demo).
• Build a map / localize / plan a path for a mobile base. — odometry here is the seed; the §7 extensions grow it into full Nav2 navigation (sessions 11–14, planning demo).
• Apply OpenCV for color detection in a robotics context. — the HSV threshold + centroid perception node (sessions 15–16, vision demo).
• Make a simulated robot sense, plan, and act. — perception (sense) → FSM (plan) → controller + twist (act): the whole loop in one package.

From line-follower to autonomous navigator

The node graph was designed so that "follow a line" is just the first behavior. Because perception, control, and behavior are decoupled, each upgrade swaps or adds one node without touching the others.

Where to read more

• Quigley, Gerkey & Smart — Programming Robots with ROS. Nodes, topics, services, actions, and cv_bridge; the everyday companion for every node above.
• Lynch & Park — Modern Robotics: Mechanics, Planning, and Control. The differential-drive kinematics of §3 and the feedback-control theory behind the PID loop.
• Siegwart, Nourbakhsh & Scaramuzza — Introduction to Autonomous Mobile Robots. Odometry, localization, mapping, and planning — the backbone of the §7 extensions.
• Siciliano & Khatib — Springer Handbook of Robotics. The sensing and machine-vision chapters behind the HSV perception node.
• OpenCV — docs.opencv.org · cvtColor, inRange, and moments for the perception pipeline.
• ROS / Nav2 — wiki.ros.org · docs.nav2.org for the navigation extension.