Building an AI Powered Follow Trolley Part 4: Software

Creating the software to control the trolley and track people, and move on to final real-world testing.

Introduction

In part three of this series, we looked at the assembly of the follow trolley mechanics and wrote a handful of Python hardware test scripts. In this article, we’ll look at the software to interface the original “FastMOT” Python application to the follow trolley, and then move on to a real-life test.

The Trolley Interface

Using our previously written test scripts as examples (and useful references) we started by writing a Python module that interfaces to our custom control board.

import RPi.GPIO as GPIO
import time

front_prox_pin = 24
rear_prox_pin = 26

motor_left_pwm_pin = 32
motor_right_pwm_pin = 33

'''
Important note on direction pins
	Setting high set direction to forward
	Setting low sets direction to reverse
'''
motor_left_direction = 29
motor_right_direction = 31

red_pin = 19
amber_pin = 21
green_pin = 23

prox_pins = [front_prox_pin, rear_prox_pin]
stacklight_pins = [red_pin, amber_pin, green_pin]
motor_pins = [motor_left_pwm_pin, motor_right_pwm_pin, motor_left_direction, motor_right_direction]

class Trolley(object):
	LIGHT_RED = 0
	LIGHT_AMBER = 1
	LIGHT_GREEN = 2

	MOTOR_LEFT = 0
	MOTOR_RIGHT = 1

	FORWARD = True
	REVERSE = False
	STOP = -1

...

The module contains only one class with multiple functions inside, each performing a specific function to control hardware on the trolley.

def __init__(self):
		super(Trolley, self).__init__()
		GPIO.setmode(GPIO.BOARD)
		GPIO.setup(prox_pins, GPIO.IN)
		GPIO.setup(stacklight_pins, GPIO.OUT, initial=GPIO.LOW)
		GPIO.setup(motor_pins, GPIO.OUT, initial=GPIO.LOW)

		self.pwmLeft = GPIO.PWM(motor_left_pwm_pin, 20000)
		self.pwmRight = GPIO.PWM(motor_right_pwm_pin, 20000)

		self.pwmLeft.start(0)
		self.pwmRight.start(0)
		
		self.frontProxBlocked = False
		self.rearProxBlocked = False

		self.motorState = Trolley.STOP

		GPIO.add_event_detect(front_prox_pin, GPIO.BOTH, callback=self._proximityCallback)
		GPIO.add_event_detect(rear_prox_pin, GPIO.BOTH, callback=self._proximityCallback)

		self.stop()
		self.setStacklight(Trolley.LIGHT_AMBER, True)

When the class is instantiated, the __init(self)__ function is called by the Python interpreter. In this function initialisation of the hardware takes place, which includes setting up the GPIO pins to their appropriate modes, enabling the two PWM channels that are used to control the motors, adding event callbacks to handle the proximity sensors and then turning on the amber stack light.

def _proximityCallback(self, channel):
		if channel == front_prox_pin:
			self.frontProxBlocked = GPIO.input(channel)

			# Check direction & stop if necessary
			if self.frontProxBlocked and self.motorState == Trolley.FORWARD:
				self.stop()

		if channel == rear_prox_pin:
			self.rearProxBlocked = GPIO.input(channel)

			# Check direction & stop if necessary
			if self.rearProxBlocked and self.motorState == Trolley.REVERSE:
				self.stop()

We utilised callbacks to handle the state changes on the proximity sensors, as this avoids the program being locked in a while loop checking inputs. When the input pin state changes, the callback is fired with the GPIO channel as an argument. This is then read within the code and fed into if statements which then determine if it was the front or rear sensor, then the direction is checked — if the trolley is moving in the direction of the sensor that was triggered then all motion is stopped.

def setMotor(self, motor, direction=True, dutyCycle=0):
		if motor == Trolley.MOTOR_LEFT:
			GPIO.output(motor_left_direction, direction)
			time.sleep(0.05)
			self.pwmLeft.ChangeDutyCycle(dutyCycle)

		if motor == Trolley.MOTOR_RIGHT:
			GPIO.output(motor_right_direction, direction)
			time.sleep(0.05)
			self.pwmRight.ChangeDutyCycle(dutyCycle)

A helper function was written to control the motors that take three parameters: motor, speed and direction. Within the function, the GPIO pin to control direction is either set high or low, for forward and reverse respectively. A small delay is then added to ensure the motor controller’s direction relay has moved before outputting a PWM value that commands the motor.

def turnLeft(self, dutyCycle=25):
		# Forklift steering - engage right motor
		if not self.frontProxBlocked or self.rearProxBlocked:
			# Stop left motor
			self.setMotor(Trolley.MOTOR_LEFT, dutyCycle=0)

			self.setMotor(Trolley.MOTOR_RIGHT, direction=Trolley.FORWARD, dutyCycle=dutyCycle)

	def turnRight(self, dutyCycle=25):
		# Forklift steering - engage left motor
		if not self.frontProxBlocked or self.rearProxBlocked:
			# Stop right motor
			self.setMotor(Trolley.MOTOR_RIGHT, dutyCycle=0)

			self.setMotor(Trolley.MOTOR_LEFT, direction=Trolley.FORWARD, dutyCycle=dutyCycle)

Additional functions were written that control steering the trolley left and right, which is a wrapper around the above function. These steering functions first check the proximity sensors to ensure neither are blocked, as the trolley has the potential to swing into someone that could be standing nearby. With both proximity sensors clear, the code continues setting the motors to the desired direction and speed.

As the trolley is rear-wheel drive, the motors have to be set in the opposite direction to the desired turn direction — to turn left, the left motor is set to reverse and the right motor set to forward, and vice-versa.

The Tracking Application

In part one, we took a look at two different person tracking applications and settled on FastMOT. With our Python trolley class in hand, we moved on to connecting the tracking to the hardware controls.

def step(self, frame):
  """
  Runs multiple object tracker on the next frame.
  Parameters
  ----------
  frame : ndarray
   The next frame.
  """
  detections = []
  if self.frame_count == 0:
   detections = self.detector(frame)
   self.tracker.initiate(frame, detections)
  else:
   if self.frame_count % self.detector_frame_skip == 0:
    tic = time.perf_counter()
    self.detector.detect_async(frame)
    self.preproc_time += time.perf_counter() - tic
    tic = time.perf_counter()
    self.tracker.compute_flow(frame)
    detections = self.detector.postprocess()
    self.detector_time += time.perf_counter() - tic
    tic = time.perf_counter()
    self.extractor.extract_async(frame, detections)
    self.tracker.apply_kalman()
    embeddings = self.extractor.postprocess()
    self.extractor_time += time.perf_counter() - tic
    tic = time.perf_counter()
    self.tracker.update(self.frame_count, detections, embeddings)
    self.association_time += time.perf_counter() - tic
    self.detector_frame_count += 1
   else:
    tic = time.perf_counter()
    self.tracker.track(frame)
    self.tracker_time += time.perf_counter() - tic

  if self.draw:
   self._draw(frame, detections)
  self.frame_count += 1

If the application is launched with the -g flag, a GUI is shown that has the live video feed from the camera with person detections overlaid on top, including a bounding box and an ID that is assigned by the identification algorithm.

while not args.gui or cv2.getWindowProperty("Video", 0) >= 0:
   frame = stream.read()
   if frame is None:
    break

   if args.mot:
    mot.step(frame)
    if log is not None:
     for track in mot.visible_tracks:
      tl = track.tlbr[:2] / config['resize_to'] * stream.resolution
      br = track.tlbr[2:] / config['resize_to'] * stream.resolution
      w, h = br - tl + 1
      log.write(f'{mot.frame_count},{track.trk_id},{tl[0]:.6f},{tl[1]:.6f},'
         f'{w:.6f},{h:.6f},-1,-1,-1\n')

   if args.gui:
    cv2.imshow('Video', frame)
    if cv2.waitKey(1) & 0xFF == 27:
     break
   if args.output_uri is not None:
    stream.write(frame)

  toc = time.perf_counter()
  elapsed_time = toc - tic

With this knowledge in hand, we started digging around in “app.py” — the main entry point for the application. The program runs in a while loop that repeatedly feeds frames into the tracker, and then displays the image on-screen with the borders added.

Tracking data is returned by the property mot.visible_tracks. To figure out what was contained in this data structure, we iterated over each track and printed it to the console. Coordinates for the top left and bottom right corners of the bounding box were included, at which point it is easy to figure out the horizontal centre of the detected person.

if mot.visible_tracks:
     t.setStacklight(Trolley.LIGHT_RED, state=True)
     # Trolley command
     for track in mot.visible_tracks:
      tl, br = tuple(track.tlbr[:2]), tuple(track.tlbr[2:])
      tx = tl[0]
      ty = tl[1]
      bx = br[0]
      by = br[1]
      mx = int(((bx - tx) / 2) + tx)
      my = int(((by - ty) / 2) + ty)
      print("ID: {}, middleX: {}, middleY: {}, motorStatus: {}".format(track.trk_id, mx, my, motorStatus))
      cv2.drawMarker(frame, (mx, my), color=(0, 255, 0), markerType=cv2.MARKER_CROSS, thickness=2)

      websocketSendData['data'].update({
       'trk_id': track.trk_id,
       'mx': mx,
       'my': my,
       })
     
      if mx > fwh+centreLimits and motorStatus == 0:
       print("Right")
       motorStatus = 1
       t.turnRight(dutyCycle=7)

      if mx < fwh-centreLimits and motorStatus == 0:
       print("Left")
       motorStatus = -1
       t.turnLeft(dutyCycle=7)

      if mx < fwh+(centreLimits/2) and motorStatus == 1 and not t.getFrontProx():
       print("Forward after right")
       motorStatus = 0
       t.forward(dutyCycle=12)

      if mx > fwh-(centreLimits/2) and motorStatus == -1 and not t.getFrontProx():
       print("Forward after left")
       motorStatus = 0
       t.forward(dutyCycle=12)
    else:
     t.setStacklight(Trolley.LIGHT_RED)
     t.stop()

With the bounding box centroid figured out, we moved on to adding additional lines to the video output as a diagnostics aid. The control logic for the trolley is simple and easily expandable — the code has a “deadband” in the centre of the frame where only forward movement takes place, providing the front proximity sensor is not blocked.

If a target person moves outside this deadband area towards the left or right, the code then checks for obstructed proximity sensors and begins to move to attempt to centre the target in the frame. Despite this being a fairly crude bang-bang algorithm to follow a person, in testing we found it to perform satisfactorily.

A better solution to control the trolley would utilise a PID loop to track a person more accurately, which would also reduce the jerkiness in the current control algorithm. Additionally, a wide-angle camera lens should be used to aid the tracking — we found that the narrow field of view makes the object tracking model and reidentification struggle somewhat.

Demonstration video

Final remarks

This project demonstrates the processing power of the NVIDIA Jetson Nano when running artificial intelligence and machine learning models, and then demonstrated an assistive use of AI to build a trolley capable of following people around.

We've published our source code to a GitHub-Repository

Articles in this series:

• Part 1: Getting Started
• Part 2: Hardware Design
• Part 3: Hardware Assembly & Test
• Part 4: Software (this article)