openpilot_comma/selfdrive/locationd/test/test_locationd.py

#!/usr/bin/env python3
import os
import json
import random
import unittest
import time
import capnp
from cffi import FFI

from cereal import log
import cereal.messaging as messaging
from cereal.services import service_list
from common.params import Params

from selfdrive.manager.process_config import managed_processes

SENSOR_DECIMATION = 1
VISION_DECIMATION = 1

LIBLOCATIOND_PATH = os.path.abspath(os.path.join(os.path.dirname(__file__), '../liblocationd.so'))


class TestLocationdLib(unittest.TestCase):
  def setUp(self):
    header = '''typedef ...* Localizer_t;
Localizer_t localizer_init();
void localizer_get_message_bytes(Localizer_t localizer, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid, char *buff, size_t buff_size);
void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t size);'''

    self.ffi = FFI()
    self.ffi.cdef(header)
    self.lib = self.ffi.dlopen(LIBLOCATIOND_PATH)

    self.localizer = self.lib.localizer_init()

    self.buff_size = 2048
    self.msg_buff = self.ffi.new(f'char[{self.buff_size}]')

  def localizer_handle_msg(self, msg_builder):
    bytstr = msg_builder.to_bytes()
    self.lib.localizer_handle_msg_bytes(self.localizer, self.ffi.from_buffer(bytstr), len(bytstr))

  def localizer_get_msg(self, t=0, inputsOK=True, sensorsOK=True, gpsOK=True, msgValid=True):
    self.lib.localizer_get_message_bytes(self.localizer, inputsOK, sensorsOK, gpsOK, msgValid, self.ffi.addressof(self.msg_buff, 0), self.buff_size)
    return log.Event.from_bytes(self.ffi.buffer(self.msg_buff), nesting_limit=self.buff_size // 8)

  def test_liblocalizer(self):
    msg = messaging.new_message('liveCalibration')
    msg.liveCalibration.validBlocks = random.randint(1, 10)
    msg.liveCalibration.rpyCalib = [random.random() / 10 for _ in range(3)]

    self.localizer_handle_msg(msg)
    liveloc = self.localizer_get_msg()
    self.assertTrue(liveloc is not None)

  @unittest.skip("temporarily disabled due to false positives")
  def test_device_fell(self):
    msg = messaging.new_message('sensorEvents', 1)
    msg.sensorEvents[0].sensor = 1
    msg.sensorEvents[0].timestamp = msg.logMonoTime
    msg.sensorEvents[0].type = 1
    msg.sensorEvents[0].init('acceleration')
    msg.sensorEvents[0].acceleration.v = [10.0, 0.0, 0.0]  # zero with gravity
    self.localizer_handle_msg(msg)

    ret = self.localizer_get_msg()
    self.assertTrue(ret.liveLocationKalman.deviceStable)

    msg = messaging.new_message('sensorEvents', 1)
    msg.sensorEvents[0].sensor = 1
    msg.sensorEvents[0].timestamp = msg.logMonoTime
    msg.sensorEvents[0].type = 1
    msg.sensorEvents[0].init('acceleration')
    msg.sensorEvents[0].acceleration.v = [50.1, 0.0, 0.0]  # more than 40 m/s**2
    self.localizer_handle_msg(msg)

    ret = self.localizer_get_msg()
    self.assertFalse(ret.liveLocationKalman.deviceStable)

  def test_posenet_spike(self):
    for _ in range(SENSOR_DECIMATION):
      msg = messaging.new_message('carState')
      msg.carState.vEgo = 6.0  # more than 5 m/s
      self.localizer_handle_msg(msg)

    ret = self.localizer_get_msg()
    self.assertTrue(ret.liveLocationKalman.posenetOK)

    for _ in range(20 * VISION_DECIMATION):  # size of hist_old
      msg = messaging.new_message('cameraOdometry')
      msg.cameraOdometry.rot = [0.0, 0.0, 0.0]
      msg.cameraOdometry.rotStd = [0.1, 0.1, 0.1]
      msg.cameraOdometry.trans = [0.0, 0.0, 0.0]
      msg.cameraOdometry.transStd = [2.0, 0.1, 0.1]
      self.localizer_handle_msg(msg)

    for _ in range(20 * VISION_DECIMATION):  # size of hist_new
      msg = messaging.new_message('cameraOdometry')
      msg.cameraOdometry.rot = [0.0, 0.0, 0.0]
      msg.cameraOdometry.rotStd = [1.0, 1.0, 1.0]
      msg.cameraOdometry.trans = [0.0, 0.0, 0.0]
      msg.cameraOdometry.transStd = [10.1, 0.1, 0.1]  # more than 4 times larger
      self.localizer_handle_msg(msg)

    ret = self.localizer_get_msg()
    self.assertFalse(ret.liveLocationKalman.posenetOK)


class TestLocationdProc(unittest.TestCase):
  MAX_WAITS = 1000
  LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'sensorEvents', 'liveCalibration']

  def setUp(self):
    random.seed(123489234)

    self.pm = messaging.PubMaster(self.LLD_MSGS)

    managed_processes['locationd'].prepare()
    managed_processes['locationd'].start()

    time.sleep(1)

  def tearDown(self):
    managed_processes['locationd'].stop()

  def send_msg(self, msg):
    self.pm.send(msg.which(), msg)
    waits_left = self.MAX_WAITS
    while waits_left and not self.pm.all_readers_updated(msg.which()):
      time.sleep(0)
      waits_left -= 1
    time.sleep(0.0001)

  def get_fake_msg(self, name, t):
    try:
      msg = messaging.new_message(name)
    except capnp.lib.capnp.KjException:
      msg = messaging.new_message(name, 0)

    if name == "gpsLocationExternal":
      msg.gpsLocationExternal.flags = 1
      msg.gpsLocationExternal.verticalAccuracy = 1.0
      msg.gpsLocationExternal.speedAccuracy = 1.0
      msg.gpsLocationExternal.bearingAccuracyDeg = 1.0
      msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0]
      msg.gpsLocationExternal.latitude = self.lat
      msg.gpsLocationExternal.longitude = self.lon
      msg.gpsLocationExternal.altitude = self.alt
    elif name == 'cameraOdometry':
      msg.cameraOdometry.rot = [0.0, 0.0, 0.0]
      msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0]
      msg.cameraOdometry.trans = [0.0, 0.0, 0.0]
      msg.cameraOdometry.transStd = [0.0, 0.0, 0.0]
    msg.logMonoTime = t
    return msg

  def test_params_gps(self):
    # first reset params
    Params().delete('LastGPSPosition')

    self.lat = 30 + (random.random() * 10.0)
    self.lon = -70 + (random.random() * 10.0)
    self.alt = 5 + (random.random() * 10.0)
    self.fake_duration = 90  # secs
    # get fake messages at the correct frequency, listed in services.py
    fake_msgs = []
    for sec in range(self.fake_duration):
      for name in self.LLD_MSGS:
        for j in range(int(service_list[name].frequency)):
          fake_msgs.append(self.get_fake_msg(name, int((sec + j / service_list[name].frequency) * 1e9)))

    for fake_msg in sorted(fake_msgs, key=lambda x: x.logMonoTime):
      self.send_msg(fake_msg)
    time.sleep(1)  # wait for async params write

    lastGPS = json.loads(Params().get('LastGPSPosition'))

    self.assertAlmostEqual(lastGPS['latitude'], self.lat, places=3)
    self.assertAlmostEqual(lastGPS['longitude'], self.lon, places=3)
    self.assertAlmostEqual(lastGPS['altitude'], self.alt, places=3)


if __name__ == "__main__":
  unittest.main()