frameworks/native: Add VR command-line tools
Bug: None
Test: `m -j32` succeeds
Change-Id: Ia83c71875eb0f207f63a168c88a138daeea42d5d
diff --git a/cmds/vr/pose/pose.cpp b/cmds/vr/pose/pose.cpp
new file mode 100644
index 0000000..2288a86
--- /dev/null
+++ b/cmds/vr/pose/pose.cpp
@@ -0,0 +1,274 @@
+// pose is a utility to query and manipulate the current pose via the pose
+// service.
+
+#include <cmath>
+#include <cstdio>
+#include <iomanip>
+#include <iostream>
+#include <regex>
+#include <vector>
+
+#include <private/dvr/types.h>
+#include <dvr/pose_client.h>
+
+using android::dvr::vec3;
+using android::dvr::quat;
+
+namespace {
+
+// Prints usage information to stderr.
+void PrintUsage(const char* executable_name) {
+ std::cerr << "Usage: " << executable_name
+ << " [--identity|--set=...|--unfreeze]\n"
+ << "\n"
+ << " no arguments: display the current pose.\n"
+ << " --identity: freeze the pose to the identity pose.\n"
+ << " --set=rx,ry,rz,rw[,px,py,pz]: freeze the pose to the given "
+ "state. rx,ry,rz,rw are interpreted as rotation quaternion. "
+ " px, py, pz as position (0,0,0 if omitted).\n"
+ << " --mode=mode: sets mode to one of normal, head_turn:slow, "
+ "head_turn:fast, rotate:slow, rotate:medium, rotate:fast, "
+ "circle_strafe.\n"
+ << " --unfreeze: sets the mode to normal.\n"
+ << " --log_controller=[true|false]: starts and stops controller"
+ " logs\n"
+ << std::endl;
+}
+
+// If return_code is negative, print out its corresponding string description
+// and exit the program with a non-zero exit code.
+void ExitIfNegative(int return_code) {
+ if (return_code < 0) {
+ std::cerr << "Error: " << strerror(-return_code) << std::endl;
+ std::exit(1);
+ }
+}
+
+// Parses the following command line flags:
+// --identity
+// --set=rx,ry,rz,rw[,px,py,pz]
+// Returns false if parsing fails.
+bool ParseState(const std::string& arg, DvrPoseState* out_state) {
+ if (arg == "--identity") {
+ *out_state = {.head_from_start_rotation = {0.f, 0.f, 0.f, 1.f},
+ .head_from_start_translation = {0.f, 0.f, 0.f},
+ .timestamp_ns = 0,
+ .sensor_from_start_rotation_velocity = {0.f, 0.f, 0.f}};
+ return true;
+ }
+
+ const std::string prefix("--set=");
+ if (arg.size() < 6 || arg.compare(0, prefix.size(), prefix) != 0) {
+ return false;
+ }
+
+ // Tokenize by ','.
+ std::regex split_by_comma("[,]+");
+ std::sregex_token_iterator token_it(arg.begin() + prefix.size(), arg.end(),
+ split_by_comma,
+ -1 /* return inbetween parts */);
+ std::sregex_token_iterator token_end;
+
+ // Convert to float and store values.
+ std::vector<float> values;
+ for (; token_it != token_end; ++token_it) {
+ std::string token = *(token_it);
+ float value = 0.f;
+ if (sscanf(token.c_str(), "%f", &value) != 1) {
+ std::cerr << "Unable to parse --set value as float: " << token
+ << std::endl;
+ return false;
+ } else {
+ values.push_back(value);
+ }
+ }
+
+ if (values.size() != 4 && values.size() != 7) {
+ std::cerr << "Unable to parse --set, expected either 4 or 7 of values."
+ << std::endl;
+ return false;
+ }
+
+ float norm2 = values[0] * values[0] + values[1] * values[1] +
+ values[2] * values[2] + values[3] * values[3];
+ if (std::abs(norm2 - 1.f) > 1e-4) {
+ if (norm2 < 1e-8) {
+ std::cerr << "--set quaternion norm close to zero." << std::endl;
+ return false;
+ }
+ float norm = std::sqrt(norm2);
+ values[0] /= norm;
+ values[1] /= norm;
+ values[2] /= norm;
+ values[3] /= norm;
+ }
+
+ out_state->head_from_start_rotation = {values[0], values[1], values[2],
+ values[3]};
+
+ if (values.size() == 7) {
+ out_state->head_from_start_translation = {values[4], values[5], values[6]};
+ } else {
+ out_state->head_from_start_translation = {0.f, 0.f, 0.f};
+ }
+
+ out_state->timestamp_ns = 0;
+ out_state->sensor_from_start_rotation_velocity = {0.f, 0.f, 0.f};
+
+ return true;
+}
+
+// Parses the command line flag --mode.
+// Returns false if parsing fails.
+bool ParseSetMode(const std::string& arg, DvrPoseMode* mode) {
+ const std::string prefix("--mode=");
+ if (arg.size() < prefix.size() ||
+ arg.compare(0, prefix.size(), prefix) != 0) {
+ return false;
+ }
+
+ std::string value = arg.substr(prefix.size());
+
+ if (value == "normal") {
+ *mode = DVR_POSE_MODE_6DOF;
+ return true;
+ } else if (value == "head_turn:slow") {
+ *mode = DVR_POSE_MODE_MOCK_HEAD_TURN_SLOW;
+ return true;
+ } else if (value == "head_turn:fast") {
+ *mode = DVR_POSE_MODE_MOCK_HEAD_TURN_FAST;
+ return true;
+ } else if (value == "rotate:slow") {
+ *mode = DVR_POSE_MODE_MOCK_ROTATE_SLOW;
+ return true;
+ } else if (value == "rotate:medium") {
+ *mode = DVR_POSE_MODE_MOCK_ROTATE_MEDIUM;
+ return true;
+ } else if (value == "rotate:fast") {
+ *mode = DVR_POSE_MODE_MOCK_ROTATE_FAST;
+ return true;
+ } else if (value == "circle_strafe") {
+ *mode = DVR_POSE_MODE_MOCK_CIRCLE_STRAFE;
+ return true;
+ } else {
+ return false;
+ }
+}
+
+// Parses the command line flag --controller_log.
+// Returns false if parsing fails.
+bool ParseLogController(const std::string& arg, bool* log_enabled) {
+ const std::string prefix("--log_controller=");
+ if (arg.size() < prefix.size() ||
+ arg.compare(0, prefix.size(), prefix) != 0) {
+ return false;
+ }
+
+ std::string value = arg.substr(prefix.size());
+
+ if (value == "false") {
+ *log_enabled = false;
+ return true;
+ } else if (value == "true") {
+ *log_enabled = true;
+ return true;
+ } else {
+ return false;
+ }
+}
+
+// The different actions that the tool can perform.
+enum class Action {
+ Query, // Query the current pose.
+ Set, // Set the pose and freeze.
+ Unfreeze, // Set the pose mode to normal.
+ SetMode, // Sets the pose mode.
+ LogController, // Start/stop controller logging in sensord.
+};
+
+// The action to perform when no arguments are passed to the tool.
+constexpr Action kDefaultAction = Action::Query;
+
+} // namespace
+
+int main(int argc, char** argv) {
+ Action action = kDefaultAction;
+ DvrPoseState state;
+ DvrPoseMode pose_mode = DVR_POSE_MODE_6DOF;
+ bool log_controller = false;
+
+ // Parse command-line arguments.
+ for (int i = 1; i < argc; ++i) {
+ const std::string arg = argv[i];
+ if (ParseState(arg, &state) && action == kDefaultAction) {
+ action = Action::Set;
+ } else if (arg == "--unfreeze" && action == kDefaultAction) {
+ action = Action::Unfreeze;
+ } else if (ParseSetMode(arg, &pose_mode) && action == kDefaultAction) {
+ action = Action::SetMode;
+ } else if (ParseLogController(arg, &log_controller)) {
+ action = Action::LogController;
+ } else {
+ PrintUsage(argv[0]);
+ return 1;
+ }
+ }
+
+ auto pose_client = dvrPoseCreate();
+ if (!pose_client) {
+ std::cerr << "Unable to create pose client." << std::endl;
+ return 1;
+ }
+
+ switch (action) {
+ case Action::Query: {
+ ExitIfNegative(dvrPosePoll(pose_client, &state));
+ uint64_t timestamp = state.timestamp_ns;
+ const auto& rotation = state.head_from_start_rotation;
+ const auto& translation = state.head_from_start_translation;
+ const auto& rotation_velocity = state.sensor_from_start_rotation_velocity;
+ quat q(rotation.w, rotation.x, rotation.y, rotation.z);
+ vec3 angles = q.matrix().eulerAngles(0, 1, 2);
+ angles = angles * 180.f / M_PI;
+ vec3 x = q * vec3(1.0f, 0.0f, 0.0f);
+ vec3 y = q * vec3(0.0f, 1.0f, 0.0f);
+ vec3 z = q * vec3(0.0f, 0.0f, 1.0f);
+
+ std::cout << "timestamp_ns: " << timestamp << std::endl
+ << "rotation_quaternion: " << rotation.x << ", " << rotation.y
+ << ", " << rotation.z << ", " << rotation.w << std::endl
+ << "rotation_angles: " << angles.x() << ", " << angles.y()
+ << ", " << angles.z() << std::endl
+ << "translation: " << translation.x << ", " << translation.y
+ << ", " << translation.z << std::endl
+ << "rotation_velocity: " << rotation_velocity.x << ", "
+ << rotation_velocity.y << ", " << rotation_velocity.z
+ << std::endl
+ << "axes: " << std::setprecision(3)
+ << "x(" << x.x() << ", " << x.y() << ", " << x.z() << "), "
+ << "y(" << y.x() << ", " << y.y() << ", " << y.z() << "), "
+ << "z(" << z.x() << ", " << z.y() << ", " << z.z() << "), "
+ << std::endl;
+ break;
+ }
+ case Action::Set: {
+ ExitIfNegative(dvrPoseFreeze(pose_client, &state));
+ break;
+ }
+ case Action::Unfreeze: {
+ ExitIfNegative(dvrPoseSetMode(pose_client, DVR_POSE_MODE_6DOF));
+ break;
+ }
+ case Action::SetMode: {
+ ExitIfNegative(dvrPoseSetMode(pose_client, pose_mode));
+ break;
+ }
+ case Action::LogController: {
+ ExitIfNegative(
+ dvrPoseLogController(pose_client, log_controller));
+ break;
+ }
+ }
+
+ dvrPoseDestroy(pose_client);
+}