Introduction

Your goal for this lab is to complete the provided sentry.py node so that it makes an audible signal any time something crosses in front of the robot. The Kobuki beeping sounds can be controlled by publishing to the /mobile_base/commands/sound topic.

Your intruder detection code should follow the algorithm below. Your solution should not contain any loops. Use appropriate numpy methods to accomplish each step.

Intruder Detection Algorithm

Complete each of the following steps on each callback:

• Extract a 1-dimensional vertical slice of depth values from the 2-dimensional depth image. Maintain both the current slice and the previous slice as separate variables:
  • \(\mathbf{c}\) - The current vertical scan
  • \(\mathbf{p}\) - vertical scan from the previous callback (store this in an instance variable)
• Calculate a distance between the two most recent scans:
  • \(d = \sum_{i=0}^{n-1} \mid c_i - p_i \mid\)

  • (This is also called the \(L1\) norm of the difference, and could be expressed \(\lVert \mathbf{c} - \mathbf{p} \rVert_1\) )

  • Note that you will need to clear out the nan values before you calculate the sum. Any sum involving a nan will be equal to nan. You can remove nan values from a numpy array using the following magic:

    x = x[~np.isnan(x)]

• Update an exponential running average of the scan differences:
  • \(average = average \times \alpha + d \times (1-\alpha)\)
• You can then signal an intruder when the ratio of the latest scan to the average scan exceeds a threshold:
  • \(\frac{d}{average} > threshold\)