Add sensor pose provider (779d1eec) · Commits · e / os / android_frameworks_av

media/libheadtracking/Android.bp

+29 −0

Original line number	Diff line number	Diff line
		@@ -22,6 +22,35 @@ cc_library {
		],
		}

		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: [

media/libheadtracking/SensorPoseProvider-example.cpp

0 → 100644

+69 −0

Original line number	Diff line number	Diff line
		/*
		* 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;
		}

media/libheadtracking/SensorPoseProvider.cpp

0 → 100644

+236 −0

Original line number	Diff line number	Diff line
		/*
		* 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

media/libheadtracking/include/media/SensorPoseProvider.h

0 → 100644

+98 −0

Original line number	Diff line number	Diff line
		/*
		* 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