Merge "Move sensord" into oc-dev

# Copyright (C) 2008 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.

LOCAL_PATH := $(call my-dir)

sourceFiles := \

  pose.cpp

staticLibraries := \

  libdvrcommon \

  libvrsensor \

  libpdx_default_transport \

sharedLibraries := \

  libcutils \

  liblog

include $(CLEAR_VARS)

LOCAL_SRC_FILES := $(sourceFiles)

LOCAL_STATIC_LIBRARIES := $(staticLibraries)

LOCAL_SHARED_LIBRARIES := $(sharedLibraries)

LOCAL_MODULE := pose

LOCAL_MODULE_TAGS := optional

include $(BUILD_EXECUTABLE)

// 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);

}

# Copyright (C) 2008 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.

LOCAL_PATH := $(call my-dir)

SENSORD_EXTEND ?= libsensordextensionstub

sourceFiles := \

	pose_service.cpp \

	sensord.cpp \

	sensor_fusion.cpp \

	sensor_hal_thread.cpp \

	sensor_ndk_thread.cpp \

	sensor_service.cpp \

	sensor_thread.cpp \

includeFiles += \

	$(LOCAL_PATH)/include

staticLibraries := \

	libdvrcommon \

	libvrsensor \

	libperformance \

	libbufferhub \

	libpdx_default_transport \

	libposepredictor \

sharedLibraries := \

	libandroid \

	libbase \

	libbinder \

	libcutils \

	liblog \

	libhardware \

	libutils \

        libui \

	$(SENSORD_EXTEND) \

cFlags := -DLOG_TAG=\"sensord\" \

          -DTRACE=0

include $(CLEAR_VARS)

LOCAL_SRC_FILES := $(sourceFiles)

LOCAL_CFLAGS := $(cFlags)

LOCAL_STATIC_LIBRARIES := $(staticLibraries)

LOCAL_SHARED_LIBRARIES := $(sharedLibraries)

LOCAL_MODULE_CLASS := EXECUTABLES

LOCAL_MODULE := sensord

LOCAL_C_INCLUDES := $(includeFiles)

LOCAL_C_INCLUDES += \

    $(call local-generated-sources-dir)/proto/frameworks/native/services/vr/sensord

LOCAL_INIT_RC := sensord.rc

include $(BUILD_EXECUTABLE)

include $(CLEAR_VARS)

LOCAL_STATIC_LIBRARIES := $(staticLibraries)

LOCAL_SHARED_LIBRARIES := $(sharedLibraries)

LOCAL_SRC_FILES := test/poselatencytest.cpp

LOCAL_MODULE := poselatencytest

include $(BUILD_EXECUTABLE)

include $(CLEAR_VARS)

LOCAL_MODULE := libsensordextensionstub

LOCAL_SRC_FILES := sensord_extension.cpp

include $(BUILD_SHARED_LIBRARY)