controlsd: use livePose (commaai#33283)

* Pose calibrator

* Fix static analysis

* Fix static

* Fix test_latcontrol

* Fix test_latcontrol

* Update services in process replay

* Fix static

* Matmul not mul

* Add assertion

* Move pose calibration to data_sample

* Update ref commit

* Remove llk from cycle alerts

* Deprecated nogps event

* Switch power_draw to lp

* Bring back noGps alert

* Add handling code back

* get_bool

* Bring inputsok back

common/mock/__init__.py

@@ -8,12 +8,12 @@
 import threading
 from cereal.messaging import PubMaster
 from cereal.services import SERVICE_LIST
-from openpilot.common.mock.generators import generate_liveLocationKalman
+from openpilot.common.mock.generators import generate_livePose
 from openpilot.common.realtime import Ratekeeper
 
 
 MOCK_GENERATOR = {
-  "liveLocationKalman": generate_liveLocationKalman
+  "livePose": generate_livePose
 }
 
 

common/mock/generators.py

@@ -1,20 +1,14 @@
 from cereal import messaging
 
 
-LOCATION1 = (32.7174, -117.16277)
-LOCATION2 = (32.7558, -117.2037)
-
-LLK_DECIMATION = 10
-RENDER_FRAMES = 15
-DEFAULT_ITERATIONS = RENDER_FRAMES * LLK_DECIMATION
-
-
-def generate_liveLocationKalman(location=LOCATION1):
-  msg = messaging.new_message('liveLocationKalman')
-  msg.liveLocationKalman.positionGeodetic = {'value': [*location, 0], 'std': [0., 0., 0.], 'valid': True}
-  msg.liveLocationKalman.positionECEF = {'value': [0., 0., 0.], 'std': [0., 0., 0.], 'valid': True}
-  msg.liveLocationKalman.calibratedOrientationNED = {'value': [0., 0., 0.], 'std': [0., 0., 0.], 'valid': True}
-  msg.liveLocationKalman.velocityCalibrated = {'value': [0., 0., 0.], 'std': [0., 0., 0.], 'valid': True}
-  msg.liveLocationKalman.status = 'valid'
-  msg.liveLocationKalman.gpsOK = True
+def generate_livePose():
+  msg = messaging.new_message('livePose')
+  meas = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'xStd': 0.0, 'yStd': 0.0, 'zStd': 0.0, 'valid': True}
+  msg.livePose.orientationNED = meas
+  msg.livePose.velocityDevice = meas
+  msg.livePose.angularVelocityDevice = meas
+  msg.livePose.accelerationDevice = meas
+  msg.livePose.inputsOK = True
+  msg.livePose.posenetOK = True
+  msg.livePose.sensorsOK = True
   return msg

selfdrive/controls/controlsd.py

@@ -28,6 +28,7 @@
 from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
 from openpilot.selfdrive.controls.lib.longcontrol import LongControl
 from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
+from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
 
 from openpilot.system.hardware import HARDWARE
 
@@ -78,6 +79,11 @@ def __init__(self, CI=None):
     # Setup sockets
     self.pm = messaging.PubMaster(['controlsState', 'carControl', 'onroadEvents'])
 
+    if self.params.get_bool("UbloxAvailable"):
+      self.gps_location_service = "gpsLocationExternal"
+    else:
+      self.gps_location_service = "gpsLocation"
+    self.gps_packets = [self.gps_location_service]
     self.sensor_packets = ["accelerometer", "gyroscope"]
     self.camera_packets = ["roadCameraState", "driverCameraState", "wideRoadCameraState"]
 
@@ -86,16 +92,16 @@ def __init__(self, CI=None):
     # TODO: de-couple controlsd with card/conflate on carState without introducing controls mismatches
     self.car_state_sock = messaging.sub_sock('carState', timeout=20)
 
-    ignore = self.sensor_packets + ['testJoystick']
+    ignore = self.sensor_packets + self.gps_packets + ['testJoystick']
     if SIMULATION:
       ignore += ['driverCameraState', 'managerState']
     if REPLAY:
       # no vipc in replay will make them ignored anyways
       ignore += ['roadCameraState', 'wideRoadCameraState']
     self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
-                                   'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman',
+                                   'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose',
                                    'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
-                                   'testJoystick'] + self.camera_packets + self.sensor_packets,
+                                   'testJoystick'] + self.camera_packets + self.sensor_packets + self.gps_packets,
                                   ignore_alive=ignore, ignore_avg_freq=ignore+['radarState', 'testJoystick'], ignore_valid=['testJoystick', ],
                                   frequency=int(1/DT_CTRL))
 
@@ -120,6 +126,9 @@ def __init__(self, CI=None):
     self.AM = AlertManager()
     self.events = Events()
 
+    self.pose_calibrator = PoseCalibrator()
+    self.calibrated_pose: Pose|None = None
+
     self.LoC = LongControl(self.CP)
     self.VM = VehicleModel(self.CP)
 
@@ -335,11 +344,9 @@ def update_events(self, CS):
       self.logged_comm_issue = None
 
     if not (self.CP.notCar and self.joystick_mode):
-      if not self.sm['liveLocationKalman'].posenetOK:
+      if not self.sm['livePose'].posenetOK:
         self.events.add(EventName.posenetInvalid)
-      if not self.sm['liveLocationKalman'].deviceStable:
-        self.events.add(EventName.deviceFalling)
-      if not self.sm['liveLocationKalman'].inputsOK:
+      if not self.sm['livePose'].inputsOK:
         self.events.add(EventName.locationdTemporaryError)
       if not self.sm['liveParameters'].valid and not TESTING_CLOSET and (not SIMULATION or REPLAY):
         self.events.add(EventName.paramsdTemporaryError)
@@ -375,10 +382,10 @@ def update_events(self, CS):
 
     # TODO: fix simulator
     if not SIMULATION or REPLAY:
-      # Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes
-      if not self.sm['liveLocationKalman'].gpsOK and self.sm['liveLocationKalman'].inputsOK and (self.distance_traveled > 1500):
+      gps_ok = self.sm.recv_frame[self.gps_location_service] > 0 and (self.sm.frame - self.sm.recv_frame[self.gps_location_service]) * DT_CTRL < 2.0
+      if not gps_ok and self.sm['livePose'].inputsOK and (self.distance_traveled > 1500):
         self.events.add(EventName.noGps)
-      if self.sm['liveLocationKalman'].gpsOK:
+      if gps_ok:
         self.distance_traveled = 0
       self.distance_traveled += CS.vEgo * DT_CTRL
 
@@ -429,6 +436,13 @@ def data_sample(self):
            if ps.safetyModel not in IGNORED_SAFETY_MODES):
       self.mismatch_counter += 1
 
+    # calibrate the live pose and save it for later use
+    if self.sm.updated["liveCalibration"]:
+      self.pose_calibrator.feed_live_calib(self.sm['liveCalibration'])
+    if self.sm.updated["livePose"]:
+      device_pose = Pose.from_live_pose(self.sm['livePose'])
+      self.calibrated_pose = self.pose_calibrator.build_calibrated_pose(device_pose)
+
     return CS
 
   def state_transition(self, CS):
@@ -573,7 +587,7 @@ def state_control(self, CS):
       actuators.curvature = self.desired_curvature
       actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
                                                                              self.steer_limited, self.desired_curvature,
-                                                                             self.sm['liveLocationKalman'])
+                                                                             self.calibrated_pose) # TODO what if not available
     else:
       lac_log = log.ControlsState.LateralDebugState.new_message()
       if self.sm.recv_frame['testJoystick'] > 0:
@@ -650,12 +664,9 @@ def publish_logs(self, CS, start_time, CC, lac_log):
 
     # Orientation and angle rates can be useful for carcontroller
     # Only calibrated (car) frame is relevant for the carcontroller
-    orientation_value = list(self.sm['liveLocationKalman'].calibratedOrientationNED.value)
-    if len(orientation_value) > 2:
-      CC.orientationNED = orientation_value
-    angular_rate_value = list(self.sm['liveLocationKalman'].angularVelocityCalibrated.value)
-    if len(angular_rate_value) > 2:
-      CC.angularVelocity = angular_rate_value
+    if self.calibrated_pose is not None:
+      CC.orientationNED = self.calibrated_pose.orientation.xyz.tolist()
+      CC.angularVelocity = self.calibrated_pose.angular_velocity.xyz.tolist()
 
     CC.cruiseControl.override = self.enabled and not CC.longActive and self.CP.openpilotLongitudinalControl
     CC.cruiseControl.cancel = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise)

selfdrive/controls/lib/latcontrol.py

@@ -17,7 +17,7 @@ def __init__(self, CP, CI):
     self.steer_max = 1.0
 
   @abstractmethod
-  def update(self, active, CS, VM, params, steer_limited, desired_curvature, llk):
+  def update(self, active, CS, VM, params, steer_limited, desired_curvature, calibrated_pose):
     pass
 
   def reset(self):

selfdrive/controls/lib/latcontrol_angle.py

@@ -11,7 +11,7 @@ def __init__(self, CP, CI):
     super().__init__(CP, CI)
     self.sat_check_min_speed = 5.
 
-  def update(self, active, CS, VM, params, steer_limited, desired_curvature, llk):
+  def update(self, active, CS, VM, params, steer_limited, desired_curvature, calibrated_pose):
     angle_log = log.ControlsState.LateralAngleState.new_message()
 
     if not active:

selfdrive/controls/lib/latcontrol_pid.py

@@ -17,7 +17,7 @@ def reset(self):
     super().reset()
     self.pid.reset()
 
-  def update(self, active, CS, VM, params, steer_limited, desired_curvature, llk):
+  def update(self, active, CS, VM, params, steer_limited, desired_curvature, calibrated_pose):
     pid_log = log.ControlsState.LateralPIDState.new_message()
     pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
     pid_log.steeringRateDeg = float(CS.steeringRateDeg)

selfdrive/controls/lib/latcontrol_torque.py

@@ -37,7 +37,7 @@ def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction):
     self.torque_params.latAccelOffset = latAccelOffset
     self.torque_params.friction = friction
 
-  def update(self, active, CS, VM, params, steer_limited, desired_curvature, llk):
+  def update(self, active, CS, VM, params, steer_limited, desired_curvature, calibrated_pose):
     pid_log = log.ControlsState.LateralTorqueState.new_message()
     if not active:
       output_torque = 0.0
@@ -49,8 +49,9 @@ def update(self, active, CS, VM, params, steer_limited, desired_curvature, llk):
         actual_curvature = actual_curvature_vm
         curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0))
       else:
-        actual_curvature_llk = llk.angularVelocityCalibrated.value[2] / CS.vEgo
-        actual_curvature = interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_llk])
+        assert calibrated_pose is not None
+        actual_curvature_pose = calibrated_pose.angular_velocity.yaw / CS.vEgo
+        actual_curvature = interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_pose])
         curvature_deadzone = 0.0
       desired_lateral_accel = desired_curvature * CS.vEgo ** 2
 

selfdrive/controls/lib/tests/test_latcontrol.py

@@ -9,7 +9,8 @@
 from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
 from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle
 from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
-from openpilot.common.mock.generators import generate_liveLocationKalman
+from openpilot.selfdrive.locationd.helpers import Pose
+from openpilot.common.mock.generators import generate_livePose
 
 
 class TestLatControl:
@@ -29,8 +30,10 @@ def test_saturation(self, car_name, controller):
 
     params = log.LiveParametersData.new_message()
 
-    llk = generate_liveLocationKalman()
+    lp = generate_livePose()
+    pose = Pose.from_live_pose(lp.livePose)
+
     for _ in range(1000):
-      _, _, lac_log = controller.update(True, CS, VM, params, False, 1, llk)
+      _, _, lac_log = controller.update(True, CS, VM, params, False, 1, pose)
 
     assert lac_log.saturated

selfdrive/debug/cycle_alerts.py

@@ -54,7 +54,7 @@ def cycle_alerts(duration=200, is_metric=False):
   CS = car.CarState.new_message()
   CP = CarInterface.get_non_essential_params("HONDA_CIVIC")
   sm = messaging.SubMaster(['deviceState', 'pandaStates', 'roadCameraState', 'modelV2', 'liveCalibration',
-                            'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman',
+                            'driverMonitoringState', 'longitudinalPlan', 'livePose',
                             'managerState'] + cameras)
 
   pm = messaging.PubMaster(['controlsState', 'pandaStates', 'deviceState'])

selfdrive/locationd/helpers.py

@@ -2,6 +2,7 @@
 from typing import Any
 
 from cereal import log
+from openpilot.common.transformations.orientation import rot_from_euler, euler_from_rot
 
 
 def rotate_cov(rot_matrix, cov_in):
@@ -70,3 +71,69 @@ def handle_log(self, t: int, which: str, msg: log.Event) -> None:
 
   def get_msg(self, valid: bool, with_points: bool) -> log.Event:
     raise NotImplementedError
+
+
+class Measurement:
+  x, y, z = (property(lambda self: self.xyz[0]), property(lambda self: self.xyz[1]), property(lambda self: self.xyz[2]))
+  x_std, y_std, z_std = (property(lambda self: self.xyz_std[0]), property(lambda self: self.xyz_std[1]), property(lambda self: self.xyz_std[2]))
+  roll, pitch, yaw = x, y, z
+  roll_std, pitch_std, yaw_std = x_std, y_std, z_std
+
+  def __init__(self, xyz: np.ndarray, xyz_std: np.ndarray):
+    self.xyz: np.ndarray = xyz
+    self.xyz_std: np.ndarray = xyz_std
+
+  @classmethod
+  def from_measurement_xyz(cls, measurement: log.LivePose.XYZMeasurement) -> 'Measurement':
+    return cls(
+      xyz=np.array([measurement.x, measurement.y, measurement.z]),
+      xyz_std=np.array([measurement.xStd, measurement.yStd, measurement.zStd])
+    )
+
+
+class Pose:
+  def __init__(self, orientation: Measurement, velocity: Measurement, acceleration: Measurement, angular_velocity: Measurement):
+    self.orientation = orientation
+    self.velocity = velocity
+    self.acceleration = acceleration
+    self.angular_velocity = angular_velocity
+
+  @classmethod
+  def from_live_pose(cls, live_pose: log.LivePose) -> 'Pose':
+    return Pose(
+      orientation=Measurement.from_measurement_xyz(live_pose.orientationNED),
+      velocity=Measurement.from_measurement_xyz(live_pose.velocityDevice),
+      acceleration=Measurement.from_measurement_xyz(live_pose.accelerationDevice),
+      angular_velocity=Measurement.from_measurement_xyz(live_pose.angularVelocityDevice)
+    )
+
+
+class PoseCalibrator:
+  def __init__(self):
+    self.calib_valid = False
+    self.calib_from_device = np.eye(3)
+
+  def _transform_calib_from_device(self, meas: Measurement):
+    new_xyz = self.calib_from_device @ meas.xyz
+    new_xyz_std = rotate_std(self.calib_from_device, meas.xyz_std)
+    return Measurement(new_xyz, new_xyz_std)
+
+  def _ned_from_calib(self, orientation: Measurement):
+    ned_from_device = rot_from_euler(orientation.xyz)
+    ned_from_calib = ned_from_device @ self.calib_from_device.T
+    ned_from_calib_euler_meas = Measurement(euler_from_rot(ned_from_calib), np.full(3, np.nan))
+    return ned_from_calib_euler_meas
+
+  def build_calibrated_pose(self, pose: Pose) -> Pose:
+    ned_from_calib_euler = self._ned_from_calib(pose.orientation)
+    angular_velocity_calib = self._transform_calib_from_device(pose.angular_velocity)
+    acceleration_calib = self._transform_calib_from_device(pose.acceleration)
+    velocity_calib = self._transform_calib_from_device(pose.angular_velocity)
+
+    return Pose(ned_from_calib_euler, velocity_calib, acceleration_calib, angular_velocity_calib)
+
+  def feed_live_calib(self, live_calib: log.LiveCalibrationData):
+    calib_rpy = np.array(live_calib.rpyCalib)
+    device_from_calib = rot_from_euler(calib_rpy)
+    self.calib_from_device = device_from_calib.T
+    self.calib_valid = live_calib.calStatus == log.LiveCalibrationData.Status.calibrated

selfdrive/locationd/paramsd.py

@@ -9,10 +9,9 @@
 from openpilot.common.params import Params
 from openpilot.common.realtime import config_realtime_process, DT_MDL
 from openpilot.common.numpy_fast import clip
-from openpilot.common.transformations.orientation import rot_from_euler
 from openpilot.selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States
 from openpilot.selfdrive.locationd.models.constants import GENERATED_DIR
-from openpilot.selfdrive.locationd.helpers import rotate_std
+from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
 from openpilot.common.swaglog import cloudlog
 
 
@@ -40,9 +39,8 @@ def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset, P_initial=No
     self.kf.filter.set_global("stiffness_rear", CP.tireStiffnessRear)
 
     self.active = False
-    self.calibrated = False
 
-    self.calib_from_device = np.eye(3)
+    self.calibrator = PoseCalibrator()
 
     self.speed = 0.0
     self.yaw_rate = 0.0
@@ -53,15 +51,12 @@ def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset, P_initial=No
 
   def handle_log(self, t, which, msg):
     if which == 'livePose':
-      angular_velocity_device = np.array([msg.angularVelocityDevice.x, msg.angularVelocityDevice.y, msg.angularVelocityDevice.z])
-      angular_velocity_device_std = np.array([msg.angularVelocityDevice.xStd, msg.angularVelocityDevice.yStd, msg.angularVelocityDevice.zStd])
-      angular_velocity_calibrated = np.matmul(self.calib_from_device, angular_velocity_device)
-      angular_velocity_calibrated_std = rotate_std(self.calib_from_device, angular_velocity_device_std)
+      device_pose = Pose.from_live_pose(msg)
+      calibrated_pose = self.calibrator.build_calibrated_pose(device_pose)
+      self.yaw_rate, self.yaw_rate_std = calibrated_pose.angular_velocity.z, calibrated_pose.angular_velocity.z_std
 
-      self.yaw_rate, self.yaw_rate_std = angular_velocity_calibrated[2], angular_velocity_calibrated_std[2]
-
-      localizer_roll = msg.orientationNED.x
-      localizer_roll_std = np.radians(1) if np.isnan(msg.orientationNED.xStd) else msg.orientationNED.xStd
+      localizer_roll, localizer_roll_std = device_pose.orientation.x, device_pose.orientation.x_std
+      localizer_roll_std = np.radians(1) if np.isnan(localizer_roll_std) else localizer_roll_std
       self.roll_valid = (localizer_roll_std < ROLL_STD_MAX) and (ROLL_MIN < localizer_roll < ROLL_MAX) and msg.sensorsOK
       if self.roll_valid:
         roll = localizer_roll
@@ -73,7 +68,7 @@ def handle_log(self, t, which, msg):
         roll_std = np.radians(10.0)
       self.roll = clip(roll, self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA)
 
-      yaw_rate_valid = msg.angularVelocityDevice.valid and self.calibrated
+      yaw_rate_valid = msg.angularVelocityDevice.valid and self.calibrator.calib_valid
       yaw_rate_valid = yaw_rate_valid and 0 < self.yaw_rate_std < 10  # rad/s
       yaw_rate_valid = yaw_rate_valid and abs(self.yaw_rate) < 1  # rad/s
 
@@ -101,9 +96,7 @@ def handle_log(self, t, which, msg):
         self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, np.array([[steer_ratio]]))
 
     elif which == 'liveCalibration':
-      self.calibrated  = msg.calStatus == log.LiveCalibrationData.Status.calibrated
-      device_from_calib = rot_from_euler(np.array(msg.rpyCalib))
-      self.calib_from_device = device_from_calib.T
+      self.calibrator.feed_live_calib(msg)
 
     elif which == 'carState':
       self.steering_angle = msg.steeringAngleDeg