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 the central column of the 2-dimensional depth image as a 1-dimensional vertical slice of depth values. You will need to 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 the L1 norm of the difference between the two most recent scans:
  • \(d = \lVert \mathbf{c} - \mathbf{p} \rVert_1 = \sum_{i=0}^{n-1} \mid c_i - p_i \mid\)

  • Note that the depth data will contain nan values. You will need to clear those out before calculating the sum or you will need to use the nan-safe versions of the aggregation methods. 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)\)
• Signal an intruder when the ratio of the latest scan to the average scan exceeds a threshold:
  • \(\frac{d}{average} > threshold\)