Add sensor pose provider
This is a small utility for wrapping the Sensor NDK APIs with a
simpler, higher-level interface, intended for providing pose
data into the head-tracking processing library.
Bug: 188502620
Test: Manual verification by running the included example binary.
Change-Id: I7ce4f351242c957321af93875f8710aec8b3bf90
diff --git a/media/libheadtracking/Android.bp b/media/libheadtracking/Android.bp
index 6141824..1a11c0e 100644
--- a/media/libheadtracking/Android.bp
+++ b/media/libheadtracking/Android.bp
@@ -22,6 +22,35 @@
],
}
+cc_library {
+ name: "libheadtracking-binding",
+ srcs: [
+ "SensorPoseProvider.cpp",
+ ],
+ shared_libs: [
+ "libheadtracking",
+ "libandroid",
+ "liblog",
+ "libsensor",
+ ],
+ export_shared_lib_headers: [
+ "libheadtracking",
+ ],
+}
+
+cc_binary {
+ name: "SensorPoseProvider-example",
+ srcs: [
+ "SensorPoseProvider-example.cpp",
+ ],
+ shared_libs: [
+ "libandroid",
+ "libheadtracking",
+ "libheadtracking-binding",
+ "libsensor",
+ ],
+}
+
cc_test_host {
name: "libheadtracking-test",
srcs: [
diff --git a/media/libheadtracking/SensorPoseProvider-example.cpp b/media/libheadtracking/SensorPoseProvider-example.cpp
new file mode 100644
index 0000000..30c6537
--- /dev/null
+++ b/media/libheadtracking/SensorPoseProvider-example.cpp
@@ -0,0 +1,69 @@
+/*
+ * 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.
+ */
+
+#include <unistd.h>
+#include <iostream>
+
+#include <android/sensor.h>
+#include <hardware/sensors.h>
+
+#include <media/SensorPoseProvider.h>
+
+using android::media::Pose3f;
+using android::media::SensorPoseProvider;
+using android::media::Twist3f;
+
+const char kPackageName[] = "SensorPoseProvider-example";
+
+class Listener : public SensorPoseProvider::Listener {
+ public:
+ void onPose(int64_t timestamp, int32_t handle, const Pose3f& pose,
+ const std::optional<Twist3f>& twist) override {
+ std::cout << "onPose t=" << timestamp << " sensor=" << handle << " pose=" << pose
+ << " twist=";
+ if (twist.has_value()) {
+ std::cout << twist.value();
+ } else {
+ std::cout << "<none>";
+ }
+ std::cout << std::endl;
+ }
+};
+
+int main() {
+ ASensorManager* sensor_manager = ASensorManager_getInstanceForPackage(kPackageName);
+ if (!sensor_manager) {
+ std::cerr << "Failed to get a sensor manager" << std::endl;
+ return 1;
+ }
+
+ const ASensor* headSensor =
+ ASensorManager_getDefaultSensor(sensor_manager, SENSOR_TYPE_GAME_ROTATION_VECTOR);
+ const ASensor* screenSensor =
+ ASensorManager_getDefaultSensor(sensor_manager, SENSOR_TYPE_ROTATION_VECTOR);
+
+ Listener listener;
+
+ std::unique_ptr<SensorPoseProvider> provider =
+ SensorPoseProvider::create(kPackageName, &listener);
+ int32_t headHandle = provider->startSensor(headSensor, std::chrono::milliseconds(500));
+ sleep(2);
+ provider->startSensor(screenSensor, std::chrono::milliseconds(500));
+ sleep(2);
+ provider->stopSensor(headHandle);
+ sleep(2);
+ return 0;
+}
diff --git a/media/libheadtracking/SensorPoseProvider.cpp b/media/libheadtracking/SensorPoseProvider.cpp
new file mode 100644
index 0000000..0142d56
--- /dev/null
+++ b/media/libheadtracking/SensorPoseProvider.cpp
@@ -0,0 +1,236 @@
+/*
+ * 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.
+ */
+
+#include <media/SensorPoseProvider.h>
+
+#define LOG_TAG "SensorPoseProvider"
+
+#include <inttypes.h>
+
+#include <future>
+#include <iostream>
+#include <map>
+#include <thread>
+
+#include <android/looper.h>
+#include <log/log_main.h>
+
+namespace android {
+namespace media {
+namespace {
+
+/**
+ * RAII-wrapper around ASensorEventQueue, which destroys it on destruction.
+ */
+class EventQueueGuard {
+ public:
+ EventQueueGuard(ASensorManager* manager, ASensorEventQueue* queue)
+ : mManager(manager), mQueue(queue) {}
+
+ ~EventQueueGuard() {
+ if (mQueue) {
+ int ret = ASensorManager_destroyEventQueue(mManager, mQueue);
+ if (ret) {
+ ALOGE("Failed to destroy event queue: %s\n", strerror(ret));
+ }
+ }
+ }
+
+ EventQueueGuard(const EventQueueGuard&) = delete;
+ EventQueueGuard& operator=(const EventQueueGuard&) = delete;
+
+ [[nodiscard]] ASensorEventQueue* get() const { return mQueue; }
+
+ private:
+ ASensorManager* const mManager;
+ ASensorEventQueue* mQueue;
+};
+
+/**
+ * RAII-wrapper around an enabled sensor, which disables it upon destruction.
+ */
+class SensorEnableGuard {
+ public:
+ SensorEnableGuard(ASensorEventQueue* queue, const ASensor* sensor)
+ : mQueue(queue), mSensor(sensor) {}
+
+ ~SensorEnableGuard() {
+ if (mSensor) {
+ int ret = ASensorEventQueue_disableSensor(mQueue, mSensor);
+ if (ret) {
+ ALOGE("Failed to disable sensor: %s\n", strerror(ret));
+ }
+ }
+ }
+
+ SensorEnableGuard(const SensorEnableGuard&) = delete;
+ SensorEnableGuard& operator=(const SensorEnableGuard&) = delete;
+
+ // Enable moving.
+ SensorEnableGuard(SensorEnableGuard&& other) : mQueue(other.mQueue), mSensor(other.mSensor) {
+ other.mSensor = nullptr;
+ }
+
+ private:
+ ASensorEventQueue* const mQueue;
+ const ASensor* mSensor;
+};
+
+/**
+ * Streams the required events to a PoseListener, based on events originating from the Sensor stack.
+ */
+class SensorPoseProviderImpl : public SensorPoseProvider {
+ public:
+ static std::unique_ptr<SensorPoseProvider> create(const char* packageName, Listener* listener) {
+ std::unique_ptr<SensorPoseProviderImpl> result(
+ new SensorPoseProviderImpl(packageName, listener));
+ return result->waitInitFinished() ? std::move(result) : nullptr;
+ }
+
+ ~SensorPoseProviderImpl() override {
+ ALooper_wake(mLooper);
+ mThread.join();
+ }
+
+ int32_t startSensor(const ASensor* sensor, std::chrono::microseconds samplingPeriod) override {
+ int32_t handle = ASensor_getHandle(sensor);
+
+ // Enable the sensor.
+ if (ASensorEventQueue_registerSensor(mQueue->get(), sensor, samplingPeriod.count(), 0)) {
+ ALOGE("Failed to enable sensor");
+ return INVALID_HANDLE;
+ }
+
+ mEnabledSensors.emplace(handle, SensorEnableGuard(mQueue->get(), sensor));
+ return handle;
+ }
+
+ void stopSensor(int handle) override { mEnabledSensors.erase(handle); }
+
+ private:
+ ALooper* mLooper;
+ Listener* const mListener;
+ std::thread mThread;
+ std::map<int32_t, SensorEnableGuard> mEnabledSensors;
+ std::unique_ptr<EventQueueGuard> mQueue;
+
+ // We must do some of the initialization operations on the worker thread, because the API relies
+ // on the thread-local looper. In addition, as a matter of convenience, we store some of the
+ // state on the stack.
+ // For that reason, we use a two-step initialization approach, where the ctor mostly just starts
+ // the worker thread and that thread would notify, via the promise below whenever initialization
+ // is finished, and whether it was successful.
+ std::promise<bool> mInitPromise;
+
+ SensorPoseProviderImpl(const char* packageName, Listener* listener)
+ : mListener(listener),
+ mThread([this, p = std::string(packageName)] { threadFunc(p.c_str()); }) {}
+
+ void initFinished(bool success) { mInitPromise.set_value(success); }
+
+ bool waitInitFinished() { return mInitPromise.get_future().get(); }
+
+ void threadFunc(const char* packageName) {
+ // The number 19 is arbitrary, only useful if using multiple objects on the same looper.
+ constexpr int kIdent = 19;
+
+ // Obtain sensor manager.
+ ASensorManager* sensor_manager = ASensorManager_getInstanceForPackage(packageName);
+ if (!sensor_manager) {
+ ALOGE("Failed to get a sensor manager");
+ initFinished(false);
+ return;
+ }
+
+ // Obtain looper.
+ mLooper = ALooper_prepare(ALOOPER_PREPARE_ALLOW_NON_CALLBACKS);
+
+ // Create event queue.
+ ASensorEventQueue* queue =
+ ASensorManager_createEventQueue(sensor_manager, mLooper, kIdent, nullptr, nullptr);
+
+ if (queue == nullptr) {
+ ALOGE("Failed to create a sensor event queue");
+ initFinished(false);
+ return;
+ }
+
+ mQueue.reset(new EventQueueGuard(sensor_manager, queue));
+
+ initFinished(true);
+
+ while (true) {
+ int ret = ALooper_pollOnce(-1 /* no timeout */, nullptr, nullptr, nullptr);
+
+ switch (ret) {
+ case ALOOPER_POLL_WAKE:
+ // Normal way to exit.
+ return;
+
+ case kIdent:
+ // Possible events on our queue.
+ break;
+
+ default:
+ ALOGE("Unexpected status out of ALooper_pollOnce: %d", ret);
+ }
+
+ // Process an event.
+ ASensorEvent event;
+ ssize_t size = ASensorEventQueue_getEvents(queue, &event, 1);
+ if (size < 0 || size > 1) {
+ ALOGE("Unexpected return value from ASensorEventQueue_getEvents: %zd", size);
+ break;
+ }
+ if (size == 0) {
+ // No events.
+ continue;
+ }
+
+ handleEvent(event);
+ }
+ }
+
+ void handleEvent(const ASensorEvent& event) {
+ auto value = parseEvent(event);
+ mListener->onPose(event.timestamp, event.sensor, std::get<0>(value), std::get<1>(value));
+ }
+
+ static std::tuple<Pose3f, std::optional<Twist3f>> parseEvent(const ASensorEvent& event) {
+ // TODO(ytai): Add more types.
+ switch (event.type) {
+ case ASENSOR_TYPE_ROTATION_VECTOR:
+ case ASENSOR_TYPE_GAME_ROTATION_VECTOR: {
+ Eigen::Quaternionf quat(event.data[3], event.data[0], event.data[1], event.data[2]);
+ return std::make_tuple(Pose3f(quat), std::optional<Twist3f>());
+ }
+
+ default:
+ ALOGE("Unsupported sensor type: %" PRId32, event.type);
+ return std::make_tuple(Pose3f(), std::optional<Twist3f>());
+ }
+ }
+};
+
+} // namespace
+
+std::unique_ptr<SensorPoseProvider> SensorPoseProvider::create(const char* packageName,
+ Listener* listener) {
+ return SensorPoseProviderImpl::create(packageName, listener);
+}
+
+} // namespace media
+} // namespace android
diff --git a/media/libheadtracking/include/media/SensorPoseProvider.h b/media/libheadtracking/include/media/SensorPoseProvider.h
new file mode 100644
index 0000000..7b79704
--- /dev/null
+++ b/media/libheadtracking/include/media/SensorPoseProvider.h
@@ -0,0 +1,98 @@
+/*
+ * 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 <memory>
+#include <optional>
+
+#include <android/sensor.h>
+
+#include "Pose.h"
+#include "Twist.h"
+
+namespace android {
+namespace media {
+
+/**
+ * A utility providing streaming of pose data from motion sensors provided by the Sensor Framework.
+ *
+ * A live instance of this interface keeps around some resources required for accessing sensor
+ * readings (e.g. a thread and a queue). Those would be released when the instance is deleted.
+ *
+ * Once alive, individual sensors can be subscribed to using startSensor() and updates can be
+ * stopped via stopSensor(). Those two methods should not be called concurrently and correct usage
+ * is assumed.
+ */
+class SensorPoseProvider {
+ public:
+ static constexpr int32_t INVALID_HANDLE = ASENSOR_INVALID;
+
+ /**
+ * Interface for consuming pose-related sensor events.
+ *
+ * The listener will be provided with a stream of events, each including:
+ * - A handle of the sensor responsible for the event.
+ * - Timestamp.
+ * - Pose.
+ * - Optional twist (time-derivative of pose).
+ *
+ * Sensors having only orientation data will have the translation part of the pose set to
+ * identity.
+ *
+ * Events are delivered in a serialized manner (i.e. callbacks do not need to be reentrant).
+ * Callbacks should not block.
+ */
+ class Listener {
+ public:
+ virtual ~Listener() = default;
+
+ virtual void onPose(int64_t timestamp, int32_t handle, const Pose3f& pose,
+ const std::optional<Twist3f>& twist) = 0;
+ };
+
+ /**
+ * Creates a new SensorPoseProvider instance.
+ * Events will be delivered to the listener as long as the returned instance is kept alive.
+ * @param packageName Client's package name.
+ * @param listener The listener that will get the events.
+ * @return The new instance, or nullptr in case of failure.
+ */
+ static std::unique_ptr<SensorPoseProvider> create(const char* packageName, Listener* listener);
+
+ virtual ~SensorPoseProvider() = default;
+
+ /**
+ * Start receiving pose updates from a given sensor.
+ * @param sensor The sensor to subscribe to.
+ * @param samplingPeriod Sampling interval, in microseconds. Actual rate might be slightly
+ * different.
+ * @return A handle, which can be later used for stopSensor(). INVALID_HANDLE would be returned
+ * in case of error.
+ */
+ virtual int32_t startSensor(const ASensor* sensor,
+ std::chrono::microseconds samplingPeriod) = 0;
+
+ /**
+ * Stop a sensor, previously started with startSensor(). It is not required to stop all sensors
+ * before deleting the SensorPoseProvider instance.
+ * @param handle The sensor handle, as returned from startSensor().
+ */
+ virtual void stopSensor(int32_t handle) = 0;
+};
+
+} // namespace media
+} // namespace android