Add SpatializerPoseController

This class encapsulates the logic for pose processing, intended for
driving a spatializer effect.

Bug: 188502620
Test: Manual testing of pose processing using logging.
Change-Id: I84d47926ff469d7c09895aba55aaf2e6c890ec00
diff --git a/services/audiopolicy/service/Android.bp b/services/audiopolicy/service/Android.bp
index 8ae0901..a73e270 100644
--- a/services/audiopolicy/service/Android.bp
+++ b/services/audiopolicy/service/Android.bp
@@ -17,6 +17,7 @@
         "AudioPolicyService.cpp",
         "CaptureStateNotifier.cpp",
         "Spatializer.cpp",
+        "SpatializerPoseController.cpp",
     ],
 
     include_dirs: [
@@ -25,6 +26,7 @@
 
     shared_libs: [
         "libactivitymanager_aidl",
+        "libandroid",
         "libaudioclient",
         "libaudioclient_aidl_conversion",
         "libaudiofoundation",
@@ -36,11 +38,14 @@
         "libcutils",
         "libeffectsconfig",
         "libhardware_legacy",
+        "libheadtracking",
+        "libheadtracking-binding",
         "liblog",
         "libmedia_helper",
         "libmediametrics",
         "libmediautils",
         "libpermission",
+        "libsensor",
         "libsensorprivacy",
         "libshmemcompat",
         "libutils",
@@ -75,6 +80,8 @@
 
     export_shared_lib_headers: [
         "libactivitymanager_aidl",
+        "libheadtracking",
+        "libheadtracking-binding",
         "libsensorprivacy",
         "framework-permission-aidl-cpp",
     ],
diff --git a/services/audiopolicy/service/SpatializerPoseController.cpp b/services/audiopolicy/service/SpatializerPoseController.cpp
new file mode 100644
index 0000000..f0d7f7c
--- /dev/null
+++ b/services/audiopolicy/service/SpatializerPoseController.cpp
@@ -0,0 +1,196 @@
+/*
+ * Copyright 2021, The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "SpatializerPoseController.h"
+
+#define LOG_TAG "VirtualizerStageController"
+//#define LOG_NDEBUG 0
+#include <utils/Log.h>
+#include <utils/SystemClock.h>
+
+namespace android {
+
+using media::createHeadTrackingProcessor;
+using media::HeadTrackingMode;
+using media::HeadTrackingProcessor;
+using media::Pose3f;
+using media::SensorPoseProvider;
+using media::Twist3f;
+
+using namespace std::chrono_literals;
+
+namespace {
+
+// This is how fast, in m/s, we allow position to shift during rate-limiting.
+constexpr auto kMaxTranslationalVelocity = 2 ;
+
+// This is how fast, in rad/s, we allow rotation angle to shift during rate-limiting.
+constexpr auto kMaxRotationalVelocity = 4 * M_PI ;
+
+// This should be set to the typical time scale that the translation sensors used drift in. This
+// means, loosely, for how long we can trust the reading to be "accurate enough". This would
+// determine the time constants used for high-pass filtering those readings. If the value is set
+// too high, we may experience drift. If it is set too low, we may experience poses tending toward
+// identity too fast.
+constexpr auto kTranslationalDriftTimeConstant = 20s;
+
+// This should be set to the typical time scale that the rotation sensors used drift in. This
+// means, loosely, for how long we can trust the reading to be "accurate enough". This would
+// determine the time constants used for high-pass filtering those readings. If the value is set
+// too high, we may experience drift. If it is set too low, we may experience poses tending toward
+// identity too fast.
+constexpr auto kRotationalDriftTimeConstant = 20s;
+
+// This is how far into the future we predict the head pose, using linear extrapolation based on
+// twist (velocity). It should be set to a value that matches the characteristic durations of moving
+// one's head. The higher we set this, the more latency we are able to reduce, but setting this too
+// high will result in high prediction errors whenever the head accelerates (changes velocity).
+constexpr auto kPredictionDuration = 10ms;
+
+// After losing this many consecutive samples from either sensor, we would treat the measurement as
+// stale;
+constexpr auto kMaxLostSamples = 4;
+
+// Time units for system clock ticks. This is what the Sensor Framework timestamps represent and
+// what we use for pose filtering.
+using Ticks = std::chrono::nanoseconds;
+
+// How many ticks in a second.
+constexpr auto kTicksPerSecond = Ticks::period::den;
+
+}  // namespace
+
+SpatializerPoseController::SpatializerPoseController(Listener* listener,
+                                                       std::chrono::microseconds sensorPeriod,
+                                                       std::chrono::microseconds maxUpdatePeriod)
+    : mListener(listener),
+      mSensorPeriod(sensorPeriod),
+      mPoseProvider(SensorPoseProvider::create("headtracker", this)),
+      mProcessor(createHeadTrackingProcessor(HeadTrackingProcessor::Options{
+              .maxTranslationalVelocity = kMaxTranslationalVelocity / kTicksPerSecond,
+              .maxRotationalVelocity = kMaxRotationalVelocity / kTicksPerSecond,
+              .translationalDriftTimeConstant = Ticks(kTranslationalDriftTimeConstant).count(),
+              .rotationalDriftTimeConstant = Ticks(kRotationalDriftTimeConstant).count(),
+              .freshnessTimeout = Ticks(sensorPeriod * kMaxLostSamples).count(),
+              .predictionDuration = Ticks(kPredictionDuration).count(),
+      })),
+      mThread([this, maxUpdatePeriod] {
+          while (true) {
+              {
+                  std::unique_lock lock(mMutex);
+                  mCondVar.wait_for(lock, maxUpdatePeriod,
+                                    [this] { return mShouldExit || mShouldCalculate; });
+                  if (mShouldExit) {
+                      ALOGV("Exiting thread");
+                      return;
+                  }
+                  calculate_l();
+                  if (!mCalculated) {
+                      mCalculated = true;
+                      mCondVar.notify_all();
+                  }
+                  mShouldCalculate = false;
+              }
+          }
+      }) {}
+
+SpatializerPoseController::~SpatializerPoseController() {
+    {
+        std::unique_lock lock(mMutex);
+        mShouldExit = true;
+        mCondVar.notify_all();
+    }
+    mThread.join();
+}
+
+void SpatializerPoseController::setHeadSensor(const ASensor* sensor) {
+    std::lock_guard lock(mMutex);
+    // Stop current sensor, if valid.
+    if (mHeadSensor != SensorPoseProvider::INVALID_HANDLE) {
+        mPoseProvider->stopSensor(mHeadSensor);
+    }
+    // Start new sensor, if valid.
+    mHeadSensor = sensor != nullptr ? mPoseProvider->startSensor(sensor, mSensorPeriod)
+                                    : SensorPoseProvider::INVALID_HANDLE;
+    mProcessor->recenter();
+}
+
+void SpatializerPoseController::setScreenSensor(const ASensor* sensor) {
+    std::lock_guard lock(mMutex);
+    // Stop current sensor, if valid.
+    if (mScreenSensor != SensorPoseProvider::INVALID_HANDLE) {
+        mPoseProvider->stopSensor(mScreenSensor);
+    }
+    // Start new sensor, if valid.
+    mScreenSensor = sensor != nullptr ? mPoseProvider->startSensor(sensor, mSensorPeriod)
+                                      : SensorPoseProvider::INVALID_HANDLE;
+    mProcessor->recenter();
+}
+
+void SpatializerPoseController::setDesiredMode(HeadTrackingMode mode) {
+    std::lock_guard lock(mMutex);
+    mProcessor->setDesiredMode(mode);
+}
+
+void SpatializerPoseController::setScreenToStagePose(const Pose3f& screenToStage) {
+    std::lock_guard lock(mMutex);
+    mProcessor->setScreenToStagePose(screenToStage);
+}
+
+void SpatializerPoseController::setDisplayOrientation(float physicalToLogicalAngle) {
+    std::lock_guard lock(mMutex);
+    mProcessor->setDisplayOrientation(physicalToLogicalAngle);
+}
+
+void SpatializerPoseController::calculateAsync() {
+    std::lock_guard lock(mMutex);
+    mShouldCalculate = true;
+    mCondVar.notify_all();
+}
+
+void SpatializerPoseController::waitUntilCalculated() {
+    std::unique_lock lock(mMutex);
+    mCondVar.wait(lock, [this] { return mCalculated; });
+}
+
+void SpatializerPoseController::calculate_l() {
+    Pose3f headToStage;
+    HeadTrackingMode mode;
+    mProcessor->calculate(elapsedRealtimeNano());
+    headToStage = mProcessor->getHeadToStagePose();
+    mode = mProcessor->getActualMode();
+    mListener->onHeadToStagePose(headToStage);
+    if (!mActualMode.has_value() || mActualMode.value() != mode) {
+        mActualMode = mode;
+        mListener->onActualModeChange(mode);
+    }
+}
+
+void SpatializerPoseController::recenter() {
+    std::lock_guard lock(mMutex);
+    mProcessor->recenter();
+}
+
+void SpatializerPoseController::onPose(int64_t timestamp, int32_t sensor, const Pose3f& pose,
+                                        const std::optional<Twist3f>& twist) {
+    std::lock_guard lock(mMutex);
+    if (sensor == mHeadSensor) {
+        mProcessor->setWorldToHeadPose(timestamp, pose, twist.value_or(Twist3f()));
+    } else if (sensor == mScreenSensor) {
+        mProcessor->setWorldToScreenPose(timestamp, pose);
+    }
+}
+
+}  // namespace android
diff --git a/services/audiopolicy/service/SpatializerPoseController.h b/services/audiopolicy/service/SpatializerPoseController.h
new file mode 100644
index 0000000..619dc7b
--- /dev/null
+++ b/services/audiopolicy/service/SpatializerPoseController.h
@@ -0,0 +1,137 @@
+/*
+ * Copyright (C) 2021 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#pragma once
+
+#include <chrono>
+#include <condition_variable>
+#include <limits>
+#include <memory>
+#include <mutex>
+#include <thread>
+
+#include <media/HeadTrackingProcessor.h>
+#include <media/SensorPoseProvider.h>
+
+namespace android {
+
+/**
+ * This class encapsulates the logic for pose processing, intended for driving a spatializer effect.
+ * This includes integration with the Sensor sub-system for retrieving sensor data, doing all the
+ * necessary processing, etc.
+ *
+ * Calculations happen on a dedicated thread and published to the client via the Listener interface.
+ * A calculation may be triggered in one of two ways:
+ * - By calling calculateAsync() - calculation will be kicked off in the background.
+ * - By setting a timeout in the ctor, a calculation will be triggered after the timeout elapsed
+ *   from the last calculateAsync() call.
+ *
+ * This class is thread-safe. Callbacks are invoked with the lock held, so it is illegal to call
+ * into this module from the callbacks.
+ */
+class SpatializerPoseController : private media::SensorPoseProvider::Listener {
+  public:
+    /**
+     * Listener interface for getting pose and mode updates.
+     * Methods will always be invoked from a designated thread. Calling into the parent class from
+     * within the callbacks is disallowed (will result in a deadlock).
+     */
+    class Listener {
+      public:
+        virtual ~Listener() = default;
+
+        virtual void onHeadToStagePose(const media::Pose3f&) = 0;
+        virtual void onActualModeChange(media::HeadTrackingMode) = 0;
+    };
+
+    /**
+     * Ctor.
+     * sensorPeriod determines how often to receive updates from the sensors (input rate).
+     * maxUpdatePeriod determines how often to produce an output when calculateAsync() isn't
+     * invoked.
+     */
+    SpatializerPoseController(Listener* listener, std::chrono::microseconds sensorPeriod,
+                               std::chrono::microseconds maxUpdatePeriod);
+
+    /** Dtor. */
+    ~SpatializerPoseController();
+
+    /**
+     * Set the sensor that is to be used for head-tracking.
+     * nullptr can be used to disable head-tracking.
+     */
+    void setHeadSensor(const ASensor* sensor);
+
+    /**
+     * Set the sensor that is to be used for screen-tracking.
+     * nullptr can be used to disable screen-tracking.
+     */
+    void setScreenSensor(const ASensor* sensor);
+
+    /** Sets the desired head-tracking mode. */
+    void setDesiredMode(media::HeadTrackingMode mode);
+
+    /**
+     * Set the screen-to-stage pose, used in all modes.
+     */
+    void setScreenToStagePose(const media::Pose3f& screenToStage);
+
+    /**
+     * Sets the display orientation.
+     * Orientation is expressed in the angle of rotation from the physical "up" side of the screen
+     * to the logical "up" side of the content displayed the screen. Counterclockwise angles, as
+     * viewed while facing the screen are positive.
+     */
+    void setDisplayOrientation(float physicalToLogicalAngle);
+
+    /**
+     * This causes the current poses for both the head and screen to be considered "center".
+     */
+    void recenter();
+
+    /**
+     * This call triggers the recalculation of the output and the invocation of the relevant
+     * callbacks. This call is async and the callbacks will be triggered shortly after.
+     */
+    void calculateAsync();
+
+    /**
+     * Blocks until calculation and invocation of the respective callbacks has happened at least
+     * once.
+     */
+    void waitUntilCalculated();
+
+  private:
+    mutable std::mutex mMutex;
+    Listener* const mListener;
+    const std::chrono::microseconds mSensorPeriod;
+    std::unique_ptr<media::SensorPoseProvider> mPoseProvider;
+    std::unique_ptr<media::HeadTrackingProcessor> mProcessor;
+    int32_t mHeadSensor = media::SensorPoseProvider::INVALID_HANDLE;
+    int32_t mScreenSensor = media::SensorPoseProvider::INVALID_HANDLE;
+    std::optional<media::HeadTrackingMode> mActualMode;
+    std::thread mThread;
+    std::condition_variable mCondVar;
+    bool mShouldCalculate = true;
+    bool mShouldExit = false;
+    bool mCalculated = false;
+
+    void onPose(int64_t timestamp, int32_t sensor, const media::Pose3f& pose,
+                const std::optional<media::Twist3f>& twist) override;
+
+    void calculate_l();
+};
+
+}  // namespace android