QXRService：基于高通QXRService获取头显SLAM Pose和IMU Data

# CMakeLists.txt
# For more information about using CMake with Android Studio, read the
# documentation: https://d.android.com/studio/projects/add-native-code.html

# Sets the minimum version of CMake required to build the native library.
#CMakeLists.txt
cmake_minimum_required(VERSION 3.4.1)

set(jni_base_dir "${CMAKE_SOURCE_DIR}/src/main/jni")
set(jniLibs_base_dir "${CMAKE_SOURCE_DIR}/src/main/jniLibs")

include_directories(${jni_base_dir}/inc)

# Creates and names a library, sets it as either STATIC
# or SHARED, and provides the relative paths to its source code.
add_library(
        #设置so文件名称.
        qxrtest
        #设置这个so文件为共享.
        SHARED
        #Provides a relative path to your source file(s).
        ${jni_base_dir}/src/qxrtest.cpp)

# Searches for a specified prebuilt library and stores the path as a
# variable. Because CMake includes system libraries in the search path by
# default, you only need to specify the name of the public NDK library
# you want to add. CMake verifies that the library exists before
# completing its build.
find_library( # Sets the name of the path variable.
        log-lib
        #Specifies the name of the NDK library that
        #you want CMake to locate.
        log)

#动态方式加载 STATIC：表示静态的.a的库 SHARED：表示.so的库。
add_library(qxrcamclient SHARED IMPORTED)
add_library(qxrcoreclient SHARED IMPORTED)
add_library(qxrsplitclient SHARED IMPORTED)

#设置要连接的so的相对路径 ${CMAKE_SOURCE_DIR}：表示CMake.txt的当前文件夹路径 
#${ANDROID_ABI}：编译时会自动根据CPU架构去选择相应的库
set_target_properties(qxrcamclient PROPERTIES
        IMPORTED_LOCATION "${jniLibs_base_dir}/${ANDROID_ABI}/libqxrcamclient.so")
set_target_properties(qxrcoreclient PROPERTIES
        IMPORTED_LOCATION "${jniLibs_base_dir}/${ANDROID_ABI}/libqxrcoreclient.so")
set_target_properties(qxrsplitclient PROPERTIES
        IMPORTED_LOCATION "${jniLibs_base_dir}/${ANDROID_ABI}/libqxrsplitclient.so")

#添加第三方头文件
target_include_directories(qxrtest PRIVATE ${jni_base_dir}/inc ${jni_base_dir}/src)

# Specifies libraries CMake should link to your target library. You
# can link multiple libraries, such as libraries you define in this
# build script, prebuilt third-party libraries, or system libraries.
target_link_libraries( # Specifies the target library.
        #制定目标库.
        qxrtest
        #Links the target library to the log library
        #included in the NDK.
        ${log-lib}
        qxrcamclient
        qxrcoreclient
        qxrsplitclient)

package com.qvr.test;

import androidx.appcompat.app.AppCompatActivity;

import android.os.Bundle;
import android.os.Handler;
import android.view.View;
import android.widget.Button;

public class MainActivity extends AppCompatActivity implements View.OnClickListener {
    private String TAG = "APP_LOG";

    private Button btnGetPose;
    private Button btnGetIMU;

    private boolean mStartGetPose = false;
    private boolean mStartGetIMU = false;

    private JNI mJni = new JNI();

    private Handler mHandler = new Handler();

    @Override
    protected void onCreate(Bundle savedInstanceState) {
        super.onCreate(savedInstanceState);
        setContentView(R.layout.activity_main);

        initView();

        //在java app加载完成native库后把 context传到native库中。
        mJni.nativeStoreContext(getApplicationContext());

        //context传入到native之后，对QXRService进行初始化
        mJni.nativeInitQxrService();
    }

    @Override
    public void onResume() {
        super.onResume();
    }

    @Override
    public void onPause() {
        super.onPause();
    }

    @Override
    public void onDestroy() {
        super.onDestroy();
    }

    public void initView() {
        btnGetPose = (Button) this.findViewById(R.id.btn_get_pose);
        btnGetPose.setOnClickListener(this);

        btnGetIMU = (Button) this.findViewById(R.id.btn_get_imu);
        btnGetIMU.setOnClickListener(this);
    }

    @Override
    public void onClick(View v) {
        switch (v.getId()) {
            case R.id.btn_get_pose: {
                if (!mStartGetPose) {
                    btnGetPose.setText("Stop-GetPose");
                    mStartGetPose = true;

                    //调用jni api,开始获取QXRService输出的SLAM Pose
                    mJni.nativeStartSavePose();
                } else {
                    btnGetPose.setText("Start-GetPose");
                    mStartGetPose = false;

                    //调用jni api,结束获取QXRService输出的SLAM Pose
                    mJni.nativeStopSavePose();
                }
            }
            break;

            case R.id.btn_get_imu: {
                if (!mStartGetIMU) {
                    btnGetIMU.setText("Stop-GetIMU");
                    mStartGetIMU = true;

                    //调用jni api,开始获取QXRService输出的IMU data
                    mJni.nativeStartGetIMU();
                } else {
                    btnGetIMU.setText("Start-GetIMU");
                    mStartGetIMU = false;

                    //调用jni api,结束获取QXRService输出的IMU data
                    mJni.nativeStopGetIMU();
                }
            }
            break;

            default:
                break;
        }
    }
}

<?xml version="1.0" encoding="utf-8"?>
<androidx.constraintlayout.widget.ConstraintLayout xmlns:android="http://schemas.android.com/apk/res/android"
    xmlns:app="http://schemas.android.com/apk/res-auto"
    xmlns:tools="http://schemas.android.com/tools"
    android:layout_width="match_parent"
    android:layout_height="match_parent"
    tools:context=".MainActivity">

    <Button
        android:id="@+id/btn_get_pose"
        android:layout_width="155dp"
        android:layout_height="60dp"
        android:layout_marginLeft="16dp"
        android:layout_marginTop="52dp"
        android:text="Start-GetPose"
        android:textAllCaps="false"
        app:layout_constraintLeft_toLeftOf="parent"
        app:layout_constraintTop_toTopOf="parent" />

    <Button
        android:id="@+id/btn_get_imu"
        android:layout_width="155dp"
        android:layout_height="60dp"
        android:layout_marginLeft="16dp"
        android:layout_marginTop="128dp"
        android:text="Start-GetIMU"
        android:textAllCaps="false"
        app:layout_constraintLeft_toLeftOf="parent"
        app:layout_constraintTop_toTopOf="parent" />

</androidx.constraintlayout.widget.ConstraintLayout>

package com.qvr.test;

import android.content.Context;

import androidx.annotation.NonNull;

public class JNI {
    {
        System.loadLibrary("qxrtest");
    }

    public native void nativeStoreContext(@NonNull Context context);

    public native void nativeInitQxrService();

    public native void nativeStartSavePose();

    public native void nativeStopSavePose();

    public native void nativeStartGetIMU();

    public native void nativeStopGetIMU();
}

/*
* Created by shawn.xiao on 2022/6/20.
*/
#include <jni.h>
#include <string>
#include <unistd.h>
#include <android/log.h>

#include <stdlib.h>
#include <stdio.h>
#include <time.h>

#include "QXRCamClient.h"
#include "QXRCoreClient.h"
#include "QVRServiceClient.h"

#include <pthread.h>
#include <fstream>
#include <iostream>
#include <typeinfo>
#include <string>
#include <cmath>

#define LOG_TAG "QVR-Test"
#define LOGW(...) __android_log_print( ANDROID_LOG_WARN, LOG_TAG, __VA_ARGS__ )

#ifdef __cplusplus
extern "C" {

//用于保存Java层传下来的Java虚拟机内存首地址和运行上下文
static struct {
    struct _JavaVM *vm;
    jobject context;
} jni_android_info;

//QXRServiceClient handler
static qvrservice_client_helper_t *qvrservice_client = NULL;

//用于保存QXRService SLAM输出Pose
static qvrservice_head_tracking_data_t *head_tracking_data = NULL;

//用于保存QXRService IMU输出数据
static qvrservice_sensor_data_raw_t *sensor_raw_data = NULL;

//用于控制抓取QXRService SLAM Pose抓取线程
bool mIsStopSavePose = false;
//用于控制抓取QXRService IMU Data抓取线程
bool mIsStopGetIMU = false;

}
#endif

/*
/* Created by shawn1.xiao on 2022/6/20.
*/
#include "qxrtest.h"

using namespace std;

#ifdef __cplusplus
extern "C" {

string txt = ".txt";
string rootPath = "/data/data/com.qvr.test/";

JNIEXPORT void JNICALL
Java_com_qvr_test_JNI_nativeStoreContext(JNIEnv *env, jobject thiz, jobject context) {
    JavaVM *jvm = nullptr;
    jint result = env->GetJavaVM(&jvm);
    assert(result == JNI_OK);
    assert(jvm);

    jobject jContext = env->NewGlobalRef(context);

    //保存JavaVM*、Context
    jni_android_info.vm = jvm;
    jni_android_info.context = jContext;
}

//Init QVRService Start
JNIEXPORT void JNICALL
Java_com_qvr_test_JNI_nativeInitQxrService(JNIEnv *env, jobject thiz) {
    int ret = QVR_ERROR;

    //*********************** QVRServiceClient Init**************************
    qvrservice_client = QXRCoreClient_Create(jni_android_info.vm, jni_android_info.context);
    if (qvrservice_client == NULL) {
        LOGW("Fail to create qvrservice_client!");
    } else {
        LOGW("Success to create qvrservice_client!");
    }

    //设置TrackingMode为6DOF
    ret = QVRServiceClient_SetTrackingMode(qvrservice_client, TRACKING_MODE_POSITIONAL);
    if (ret != QVR_SUCCESS) {
        LOGW("set tracking mode 6dof failed!  ret:%d", ret);
    }

    sleep(1);

    //获取当前VRMode，必须要是VRMODE_STOPPED状态才能Start
    QVRSERVICE_VRMODE_STATE vrmode = QVRServiceClient_GetVRMode(qvrservice_client);
    LOGW("get vr mode:%d", vrmode);
    if (VRMODE_STOPPED == vrmode) {
        //当前VRMode为VRMODE_STOPPED状态下，Start VRMode
        ret = QVRServiceClient_StartVRMode(qvrservice_client);
        LOGW("start vr mode ret:%d, vrmode:%d", ret, QVRServiceClient_GetVRMode(qvrservice_client));
        if (ret != QVR_SUCCESS) {
            LOGW("start vr mode failed");
        }
    }
}
//Init QVRService End

//获取当前系统时间，按format进行转换
string getDateTime() {  //24H data format
    struct tm tm;
    time_t ts = time(0);
    localtime_r(&ts, &tm);

    char buff[128];
    strftime(buff, sizeof(buff), "%Y-%m%d-%H%M-%S", &tm);

    string time = buff;
    return time;
}

//GetPose Start
//抓取QXRService SLAM Pose数据线程
void *savePoseThread(void *arg) {
    int ret = QVR_ERROR;
    mIsStopSavePose = false;

    //用于保存每条Pose
    string trajContent;
    //文件全路径为：根目录路径+"traj-"+当前时间+".txt"
    string trajPath = rootPath + "traj-" + getDateTime() + txt;

    ofstream os_traj;                   //创建文件输出流对象
    os_traj.open(trajPath, ios::app);   //将对象与文件关联

    //while循环获取，当mIsStopSavePose为true时，跳出循环
    while (!mIsStopSavePose) {
        //通过创建的qvrservice_client获取Pose数据
        ret = QVRServiceClient_GetHeadTrackingData(qvrservice_client, &head_tracking_data);
        if (ret == QVR_SUCCESS && head_tracking_data != NULL) {
            //每一条Pose包含：{时间戳，Position，四元素}
            //四元素可以自行转换欧拉角，相关函数已实现，此处不作说明
            trajContent = to_string(head_tracking_data->ts)
                          + " " + to_string(head_tracking_data->translation[0])
                          + " " + to_string(head_tracking_data->translation[1])
                          + " " + to_string(head_tracking_data->translation[2])
                          + " " + to_string(head_tracking_data->rotation[0])
                          + " " + to_string(head_tracking_data->rotation[1])
                          + " " + to_string(head_tracking_data->rotation[2])
                          + " " + to_string(head_tracking_data->rotation[3])
                          + "\n";
            os_traj << trajContent;
        }
        usleep(10000);
    }

    sleep(1);
    os_traj.close();

    pthread_exit(NULL);
    return NULL;
}

JNIEXPORT void JNICALL
Java_com_qvr_test_JNI_nativeStartSavePose(JNIEnv *env, jobject obj) {
    //创建抓取Pose数据线程
    pthread_t myThread;
    int res = pthread_create(&myThread, NULL, savePoseThread, NULL);
    if (res != 0) {
        LOGW("savePoseThread create failed!");
        return;
    }
}

JNIEXPORT void JNICALL
Java_com_qvr_test_JNI_nativeStopSavePose(JNIEnv *env, jobject obj) {
    LOGW("nativeStopSavePose()    mIsStopSavePose:%d", mIsStopSavePose);
    //由Java调用，Stop的时候，线程停止运行
    mIsStopSavePose = true;
}
//GetPose End

//GetImu Start
//流程与PoseThread类似，仅差异部分作注解，其他部分请看看代码就明白了
void *saveImuThread(void *arg) {
    int ret = QVR_ERROR;
    mIsStopGetIMU = false;

    //IMU 陀螺仪和加速度计数据
    string gyroContent, acceContent;
    string gyroPath = rootPath + "Gyro-" + getDateTime() + txt;
    string accePath = rootPath + "Acce-" + getDateTime() + txt;

    ofstream os_gyro, os_acce;
    os_gyro.open(gyroPath, ios::app);
    os_acce.open(accePath, ios::app);

    while (!mIsStopGetIMU) {
        ret = QVRServiceClient_GetSensorRawData(qvrservice_client, &sensor_raw_data);
        if (ret == QVR_SUCCESS && sensor_raw_data != NULL) {
            LOGW("GetIMU Gyroscope     :{%lu, %f, %f, %f}", sensor_raw_data->gts,sensor_raw_data->gx,sensor_raw_data->gy,sensor_raw_data->gz);
            LOGW("GetIMU Accelerometer :{%lu, %f, %f, %f}", sensor_raw_data->ats,sensor_raw_data->ax,sensor_raw_data->ay,sensor_raw_data->az);
            gyroContent = to_string(sensor_raw_data->gts)
                          + " " + to_string(sensor_raw_data->gx)
                          + " " + to_string(sensor_raw_data->gy)
                          + " " + to_string(sensor_raw_data->gz)
                          + "\n";
            os_gyro << gyroContent;

            acceContent = to_string(sensor_raw_data->ats)
                          + " " + to_string(sensor_raw_data->ax)
                          + " " + to_string(sensor_raw_data->ay)
                          + " " + to_string(sensor_raw_data->az)
                          + "\n";
            os_acce << acceContent;
        }

        usleep(10000);
    }

    sleep(1);
    os_gyro.close();
    os_acce.close();

    pthread_exit(NULL);
    return NULL;
}

JNIEXPORT void JNICALL
Java_com_qvr_test_JNI_nativeStartGetIMU(JNIEnv *env, jobject obj) {
    pthread_t myThread;
    int res = pthread_create(&myThread, NULL, saveImuThread, NULL);
    if (res != 0) {
        LOGW("saveImuThread create failed!");
        return;
    }
}

JNIEXPORT void JNICALL
Java_com_qvr_test_JNI_nativeStopGetIMU(JNIEnv *env, jobject obj) {
       mIsStopGetIMU = true;
}
//GetImu End

}
#endif