cmds/vr/pose/pose.cpp - android_frameworks_native - Gitiles

 // 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, float, motion_sickness.\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 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 {
     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);
 }
	// 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, float, motion_sickness.\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 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 {
	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);
	}