Interesting pose modes (78ac0c5c) · Commits · e / os / platform_frameworks_native

cmds/vr/pose/pose.cpp

+7 −1

Original line number	Original line	Diff line number	Diff line
	@@ -28,7 +28,7 @@ void PrintUsage(const char* executable_name) {
	" px, py, pz as position (0,0,0 if omitted).\n"		" px, py, pz as position (0,0,0 if omitted).\n"
	<< " --mode=mode: sets mode to one of normal, head_turn:slow, "		<< " --mode=mode: sets mode to one of normal, head_turn:slow, "
	"head_turn:fast, rotate:slow, rotate:medium, rotate:fast, "		"head_turn:fast, rotate:slow, rotate:medium, rotate:fast, "
	"circle_strafe.\n"		"circle_strafe, float, motion_sickness.\n"
	<< " --unfreeze: sets the mode to normal.\n"		<< " --unfreeze: sets the mode to normal.\n"
	<< " --log_controller=[true\|false]: starts and stops controller"		<< " --log_controller=[true\|false]: starts and stops controller"
	" logs\n"		" logs\n"
	@@ -150,6 +150,12 @@ bool ParseSetMode(const std::string& arg, DvrPoseMode* mode) {
	} else if (value == "circle_strafe") {		} else if (value == "circle_strafe") {
	*mode = DVR_POSE_MODE_MOCK_CIRCLE_STRAFE;		*mode = DVR_POSE_MODE_MOCK_CIRCLE_STRAFE;
	return true;		return true;
			} else if (value == "float") {
			*mode = DVR_POSE_MODE_FLOAT;
			return true;
			} else if (value == "motion_sickness") {
			*mode = DVR_POSE_MODE_MOCK_MOTION_SICKNESS;
			return true;
	} else {		} else {
	return false;		return false;
	}		}

libs/vr/libvrsensor/include/dvr/pose_client.h

+2 −0

Original line number	Original line	Diff line number	Diff line
	@@ -105,6 +105,8 @@ typedef enum DvrPoseMode {
	DVR_POSE_MODE_MOCK_ROTATE_MEDIUM,		DVR_POSE_MODE_MOCK_ROTATE_MEDIUM,
	DVR_POSE_MODE_MOCK_ROTATE_FAST,		DVR_POSE_MODE_MOCK_ROTATE_FAST,
	DVR_POSE_MODE_MOCK_CIRCLE_STRAFE,		DVR_POSE_MODE_MOCK_CIRCLE_STRAFE,
			DVR_POSE_MODE_FLOAT,
			DVR_POSE_MODE_MOCK_MOTION_SICKNESS,

	// Always last.		// Always last.
	DVR_POSE_MODE_COUNT,		DVR_POSE_MODE_COUNT,

services/vr/sensord/pose_service.cpp

+56 −8

Original line number	Original line	Diff line number	Diff line
	@@ -91,6 +91,10 @@ std::string GetPoseModeString(DvrPoseMode mode) {
	return "DVR_POSE_MODE_MOCK_ROTATE_FAST";		return "DVR_POSE_MODE_MOCK_ROTATE_FAST";
	case DVR_POSE_MODE_MOCK_CIRCLE_STRAFE:		case DVR_POSE_MODE_MOCK_CIRCLE_STRAFE:
	return "DVR_POSE_MODE_MOCK_CIRCLE_STRAFE";		return "DVR_POSE_MODE_MOCK_CIRCLE_STRAFE";
			case DVR_POSE_MODE_FLOAT:
			return "DVR_POSE_MODE_FLOAT";
			case DVR_POSE_MODE_MOCK_MOTION_SICKNESS:
			return "DVR_POSE_MODE_MOCK_MOTION_SICKNESS";
	default:		default:
	return "Unknown pose mode";		return "Unknown pose mode";
	}		}
	@@ -418,7 +422,8 @@ void PoseService::UpdatePoseMode() {
	current_time_ns);		current_time_ns);
	break;		break;
	}		}
	case DVR_POSE_MODE_3DOF: {		case DVR_POSE_MODE_3DOF:
			case DVR_POSE_MODE_FLOAT: {
	// Sensor fusion provides IMU-space data, transform to world space.		// Sensor fusion provides IMU-space data, transform to world space.

	// Constants to perform IMU orientation adjustments. Note that these		// Constants to perform IMU orientation adjustments. Note that these
	@@ -446,13 +451,25 @@ void PoseService::UpdatePoseMode() {
	}		}
	start_from_head_rotation.normalize();		start_from_head_rotation.normalize();

			Vector3d position;
			switch (pose_mode_) {
			default:
			case DVR_POSE_MODE_3DOF:
	// Neck / head model code procedure for when no 6dof is available.		// Neck / head model code procedure for when no 6dof is available.
	// To apply the neck model, first translate the head pose to the new		// To apply the neck model, first translate the head pose to the new
	// center of eyes, then rotate around the origin (the original head		// center of eyes, then rotate around the origin (the original head
	// pos).		// pos).
	Vector3d position =		position = start_from_head_rotation *
	start_from_head_rotation * Vector3d(0.0, kDefaultNeckVerticalOffset,		Vector3d(0.0, kDefaultNeckVerticalOffset,
	-kDefaultNeckHorizontalOffset);		-kDefaultNeckHorizontalOffset);
			break;
			case DVR_POSE_MODE_FLOAT:
			// Change position a bit in facing direction.
			mock_pos_offset_ += start_from_head_rotation.toRotationMatrix() * Vector3d(0, 0, -0.01);
			ResetMockDeviatedPosition();
			position = mock_pos_offset_;
			break;
			}

	// IMU driver gives timestamps on its own clock, but we need monotonic		// IMU driver gives timestamps on its own clock, but we need monotonic
	// clock. Subtract 5ms to account for estimated IMU sample latency.		// clock. Subtract 5ms to account for estimated IMU sample latency.
	@@ -460,6 +477,26 @@ void PoseService::UpdatePoseMode() {
	pose_state.timestamp_ns + 5000000);		pose_state.timestamp_ns + 5000000);
	break;		break;
	}		}
			case DVR_POSE_MODE_MOCK_MOTION_SICKNESS: {
			double phase = std::sin(current_time_ns / 1e9) + 1;
			// Randomize 3rd order rotation axis on phase minimum.
			if (phase > mock_prev_phase_ && mock_diff_phase_ < 0)
			mock_rot_axis_2_ = RandVector();
			mock_diff_phase_ = phase - mock_prev_phase_;
			mock_prev_phase_ = phase;

			// Rotate axes all the way down.
			mock_rot_axis_2_ = AngleAxisd(0.004 * phase, mock_rot_axis_3_) * mock_rot_axis_2_;
			mock_rot_axis_1_ = AngleAxisd(0.002 * (std::sin(current_time_ns / 5e8 + M_PI / 2) + 1), mock_rot_axis_2_) * mock_rot_axis_1_;
			Rotationd rotation = Rotationd(AngleAxisd(fmod(current_time_ns / 2e9, kTwoPi), mock_rot_axis_1_));

			// Change position a bit.
			mock_pos_offset_ += rotation.toRotationMatrix() * Vector3d(0, 0, 0.003 * (std::sin(current_time_ns / 6e8) + 1));
			ResetMockDeviatedPosition();

			WriteAsyncPoses(mock_pos_offset_, rotation, current_time_ns);
			break;
			}
	default:		default:
	case DVR_POSE_MODE_6DOF:		case DVR_POSE_MODE_6DOF:
	ALOGE("ERROR: invalid pose mode");		ALOGE("ERROR: invalid pose mode");
	@@ -467,6 +504,13 @@ void PoseService::UpdatePoseMode() {
	}		}
	}		}

			void PoseService::ResetMockDeviatedPosition() {
			if (mock_pos_offset_[1] < -1) mock_pos_offset_[1] = 2;
			if (mock_pos_offset_[1] > 30) mock_pos_offset_[1] = 2;
			if (abs(mock_pos_offset_[0]) > 30) mock_pos_offset_[0] = mock_pos_offset_[2] = 0;
			if (abs(mock_pos_offset_[2]) > 30) mock_pos_offset_[0] = mock_pos_offset_[2] = 0;
			}

	int PoseService::HandleMessage(pdx::Message& msg) {		int PoseService::HandleMessage(pdx::Message& msg) {
	int ret = 0;		int ret = 0;
	const pdx::MessageInfo& info = msg.GetInfo();		const pdx::MessageInfo& info = msg.GetInfo();
	@@ -646,6 +690,10 @@ void PoseService::SetPoseMode(DvrPoseMode mode) {
	if (mode == DVR_POSE_MODE_6DOF) {		if (mode == DVR_POSE_MODE_6DOF) {
	// Only 3DoF is currently supported.		// Only 3DoF is currently supported.
	mode = DVR_POSE_MODE_3DOF;		mode = DVR_POSE_MODE_3DOF;
			} else if (mode == DVR_POSE_MODE_MOCK_MOTION_SICKNESS) {
			mock_rot_axis_1_ = RandVector();
			mock_rot_axis_2_ = RandVector();
			mock_rot_axis_3_ = RandVector();
	}		}

	pose_mode_ = mode;		pose_mode_ = mode;

services/vr/sensord/pose_service.h

+19 −0

Original line number	Original line	Diff line number	Diff line
	@@ -99,6 +99,25 @@ class PoseService : public pdx::ServiceBase<PoseService> {
	// Last known pose.		// Last known pose.
	DvrPoseAsync last_known_pose_;		DvrPoseAsync last_known_pose_;

			// Position offset for use in pose modes.
			Eigen::Vector3d mock_pos_offset_;

			// Phase data for DVR_POSE_MODE_MOCK_MOTION_SICKNESS.
			double mock_prev_phase_, mock_diff_phase_;

			// Axis data for DVR_POSE_MODE_MOCK_MOTION_SICKNESS.
			Eigen::Vector3d mock_rot_axis_1_, mock_rot_axis_2_, mock_rot_axis_3_;

			// Return a random normalized 3d vector.
			static Eigen::Vector3d RandVector() {
			Eigen::Vector3d vec = Eigen::Vector3d::Random();
			vec.normalize();
			return vec;
			}

			// Reset mock_pos_offset_ if strayed too far
			void ResetMockDeviatedPosition();

	// If this flag is true, the pose published includes a small prediction of		// If this flag is true, the pose published includes a small prediction of
	// where it'll be when it's consumed.		// where it'll be when it's consumed.
	bool enable_pose_prediction_;		bool enable_pose_prediction_;