Java tutorial
/* * Copyright 2014 Google Inc. All Rights Reserved. * * 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. */ package com.projecttango.examples.java.pointcloud; import com.google.atap.tango.ux.TangoUx; import com.google.atap.tango.ux.TangoUx.StartParams; import com.google.atap.tango.ux.TangoUxLayout; import com.google.atap.tango.ux.UxExceptionEvent; import com.google.atap.tango.ux.UxExceptionEventListener; import com.google.atap.tangoservice.Tango; import com.google.atap.tangoservice.Tango.OnTangoUpdateListener; import com.google.atap.tangoservice.TangoConfig; import com.google.atap.tangoservice.TangoCoordinateFramePair; import com.google.atap.tangoservice.TangoErrorException; import com.google.atap.tangoservice.TangoEvent; import com.google.atap.tangoservice.TangoInvalidException; import com.google.atap.tangoservice.TangoOutOfDateException; import com.google.atap.tangoservice.TangoPointCloudData; import com.google.atap.tangoservice.TangoPoseData; import com.google.atap.tangoservice.TangoXyzIjData; import android.Manifest; import android.app.Activity; import android.hardware.display.DisplayManager; import android.opengl.GLSurfaceView; import android.os.Bundle; import android.support.v4.app.ActivityCompat; import android.support.v4.content.ContextCompat; import android.util.Log; import android.view.Display; import android.view.MotionEvent; import android.view.View; import android.widget.TextView; import android.widget.Toast; /* FOR TEXT TO SPEECH */ import android.speech.tts.TextToSpeech; import org.rajawali3d.scene.ASceneFrameCallback; import org.rajawali3d.surface.RajawaliSurfaceView; import java.nio.FloatBuffer; import java.text.DecimalFormat; import java.util.ArrayList; import java.util.Locale; import com.projecttango.tangosupport.TangoPointCloudManager; import com.projecttango.tangosupport.TangoSupport; /** * Main Activity class for the Point Cloud Sample. Handles the connection to the {@link Tango} * service and propagation of Tango PointCloud data to OpenGL and Layout views. OpenGL rendering * logic is delegated to the {@link PointCloudRajawaliRenderer} class. */ public class PointCloudActivity extends Activity { private static final String TAG = PointCloudActivity.class.getSimpleName(); private static final int SECS_TO_MILLISECS = 1000; private static final String CAMERA_PERMISSION = Manifest.permission.CAMERA; private static final int CAMERA_PERMISSION_CODE = 0; private Tango mTango; private TangoConfig mConfig; private TangoUx mTangoUx; private TangoPointCloudManager mPointCloudManager; private PointCloudRajawaliRenderer mRenderer; private RajawaliSurfaceView mSurfaceView; private TextView mPointCountTextView; private TextView mAverageZTextView; private TextToSpeech tts; // for text-to-speech private double mPointCloudPreviousTimeStamp; private double ttsPreviousAlertTimeStamp; private boolean mIsConnected = false; private static final DecimalFormat FORMAT_THREE_DECIMAL = new DecimalFormat("0.000"); private static final double UPDATE_INTERVAL_MS = 100.0; private double mPointCloudTimeToNextUpdate = UPDATE_INTERVAL_MS; private int mDisplayRotation = 0; @Override protected void onCreate(Bundle savedInstanceState) { super.onCreate(savedInstanceState); setContentView(R.layout.activity_point_cloud); mPointCountTextView = (TextView) findViewById(R.id.point_count_textview); mAverageZTextView = (TextView) findViewById(R.id.average_z_textview); mSurfaceView = (RajawaliSurfaceView) findViewById(R.id.gl_surface_view); mPointCloudManager = new TangoPointCloudManager(); mTangoUx = setupTangoUxAndLayout(); mRenderer = new PointCloudRajawaliRenderer(this); setupRenderer(); /* Setup tts */ tts = new TextToSpeech(getApplicationContext(), new TextToSpeech.OnInitListener() { @Override public void onInit(int status) { if (status == TextToSpeech.SUCCESS) { tts.setLanguage(Locale.US); tts.speak("ICU helper initialized.", TextToSpeech.QUEUE_FLUSH, null); } } }); DisplayManager displayManager = (DisplayManager) getSystemService(DISPLAY_SERVICE); if (displayManager != null) { displayManager.registerDisplayListener(new DisplayManager.DisplayListener() { @Override public void onDisplayAdded(int displayId) { } @Override public void onDisplayChanged(int displayId) { synchronized (this) { setDisplayRotation(); } } @Override public void onDisplayRemoved(int displayId) { } }, null); } } @Override protected void onStart() { super.onStart(); mSurfaceView.onResume(); mTangoUx.start(new StartParams()); // Check and request camera permission at run time. bindTangoService(); } @Override protected void onStop() { super.onStop(); // Synchronize against disconnecting while the service is being used in the OpenGL // thread or in the UI thread. // NOTE: DO NOT lock against this same object in the Tango callback thread. // Tango.disconnect will block here until all Tango callback calls are finished. // If you lock against this object in a Tango callback thread it will cause a deadlock. synchronized (this) { if (mIsConnected) { try { mTangoUx.stop(); mTango.disconnect(); mIsConnected = false; /* Shut down tts */ if (tts != null) { tts.stop(); tts.shutdown(); } } catch (TangoErrorException e) { Log.e(TAG, getString(R.string.exception_tango_error), e); } } } } /** * Initialize Tango Service as a normal Android Service. */ private void bindTangoService() { // Initialize Tango Service as a normal Android Service, since we call mTango.disconnect() // in onPause, this will unbind Tango Service, so every time when onResume gets called, we // should create a new Tango object. mTango = new Tango(PointCloudActivity.this, new Runnable() { // Pass in a Runnable to be called from UI thread when Tango is ready, this Runnable // will be running on a new thread. // When Tango is ready, we can call Tango functions safely here only when there is no UI // thread changes involved. @Override public void run() { // Synchronize against disconnecting while the service is being used in the OpenGL // thread or in the UI thread. synchronized (PointCloudActivity.this) { try { TangoSupport.initialize(); mConfig = setupTangoConfig(mTango); mTango.connect(mConfig); startupTango(); mIsConnected = true; setDisplayRotation(); } catch (TangoOutOfDateException e) { if (mTangoUx != null) { mTangoUx.showTangoOutOfDate(); } Log.e(TAG, getString(R.string.exception_out_of_date), e); } catch (TangoErrorException e) { Log.e(TAG, getString(R.string.exception_tango_error), e); showsToastAndFinishOnUiThread(R.string.exception_tango_error); } catch (TangoInvalidException e) { Log.e(TAG, getString(R.string.exception_tango_invalid), e); showsToastAndFinishOnUiThread(R.string.exception_tango_invalid); } } } }); } /** * Sets up the tango configuration object. Make sure mTango object is initialized before * making this call. */ private TangoConfig setupTangoConfig(Tango tango) { // Use the default configuration plus add depth sensing. TangoConfig config = tango.getConfig(TangoConfig.CONFIG_TYPE_DEFAULT); config.putBoolean(TangoConfig.KEY_BOOLEAN_DEPTH, true); config.putInt(TangoConfig.KEY_INT_DEPTH_MODE, TangoConfig.TANGO_DEPTH_MODE_POINT_CLOUD); return config; } /** * Set up the callback listeners for the Tango service and obtain other parameters required * after Tango connection. * Listen to updates from the Point Cloud and Tango Events and Pose. */ private void startupTango() { ArrayList<TangoCoordinateFramePair> framePairs = new ArrayList<TangoCoordinateFramePair>(); framePairs.add(new TangoCoordinateFramePair(TangoPoseData.COORDINATE_FRAME_START_OF_SERVICE, TangoPoseData.COORDINATE_FRAME_DEVICE)); mTango.connectListener(framePairs, new OnTangoUpdateListener() { @Override public void onPoseAvailable(TangoPoseData pose) { // Passing in the pose data to UX library produce exceptions. if (mTangoUx != null) { mTangoUx.updatePoseStatus(pose.statusCode); } } @Override public void onXyzIjAvailable(TangoXyzIjData xyzIj) { // We are not using onXyzIjAvailable for this app. } @Override public void onPointCloudAvailable(TangoPointCloudData pointCloud) { if (mTangoUx != null) { mTangoUx.updatePointCloud(pointCloud); } mPointCloudManager.updatePointCloud(pointCloud); final double currentTimeStamp = pointCloud.timestamp; final double pointCloudFrameDelta = (currentTimeStamp - mPointCloudPreviousTimeStamp) * SECS_TO_MILLISECS; final double ttsAlertTimeDelta = (currentTimeStamp - ttsPreviousAlertTimeStamp) * SECS_TO_MILLISECS; mPointCloudPreviousTimeStamp = currentTimeStamp; final double averageDepth = getAveragedDepth(pointCloud.points, pointCloud.numPoints); mPointCloudTimeToNextUpdate -= pointCloudFrameDelta; if (mPointCloudTimeToNextUpdate < 0.0) { mPointCloudTimeToNextUpdate = UPDATE_INTERVAL_MS; final String pointCountString = Integer.toString(pointCloud.numPoints); runOnUiThread(new Runnable() { @Override public void run() { mPointCountTextView.setText(pointCountString); mAverageZTextView.setText(FORMAT_THREE_DECIMAL.format(averageDepth)); } }); } double MIN_TRACKING_METERS = 0.50; double ARM_LENGTH_METERS = 1.42; double WAIT_TIME_MILLISECS = 5000.0; // five seconds // double MIN_X_METERS = -0.05; // double MAX_X_METERS = 0.05 // double MIN_Y_METERS = -0.5 // double MAX_Y_METERS = 0.5; if (MIN_TRACKING_METERS <= averageDepth && averageDepth <= ARM_LENGTH_METERS && ttsAlertTimeDelta >= WAIT_TIME_MILLISECS) { if (!tts.isSpeaking()) { ttsPreviousAlertTimeStamp = currentTimeStamp; String warning = "There is an object ahead of you within arms length."; tts.speak(warning, TextToSpeech.QUEUE_FLUSH, null); } } float[] averagedXY = getAveragedXY(pointCloud.points, pointCloud.numPoints); double averagedX = averagedXY[0]; double averagedY = averagedXY[1]; System.out.println("avg (x,y) : " + "(" + averagedX + ", " + averagedY + ")"); } @Override public void onTangoEvent(TangoEvent event) { if (mTangoUx != null) { mTangoUx.updateTangoEvent(event); } } @Override public void onFrameAvailable(int cameraId) { // We are not using onFrameAvailable for this application. } }); } /** * Sets Rajawali surface view and its renderer. This is ideally called only once in onCreate. */ public void setupRenderer() { mSurfaceView.setEGLContextClientVersion(2); mRenderer.getCurrentScene().registerFrameCallback(new ASceneFrameCallback() { @Override public void onPreFrame(long sceneTime, double deltaTime) { // NOTE: This will be executed on each cycle before rendering, called from the // OpenGL rendering thread // Prevent concurrent access from a service disconnect through the onPause event. synchronized (PointCloudActivity.this) { // Don't execute any tango API actions if we're not connected to the service. if (!mIsConnected) { return; } // Update point cloud data. TangoPointCloudData pointCloud = mPointCloudManager.getLatestPointCloud(); if (pointCloud != null) { // Calculate the camera color pose at the camera frame update time in // OpenGL engine. TangoSupport.TangoMatrixTransformData transform = TangoSupport.getMatrixTransformAtTime( pointCloud.timestamp, TangoPoseData.COORDINATE_FRAME_START_OF_SERVICE, TangoPoseData.COORDINATE_FRAME_CAMERA_DEPTH, TangoSupport.TANGO_SUPPORT_ENGINE_OPENGL, TangoSupport.TANGO_SUPPORT_ENGINE_TANGO, TangoSupport.ROTATION_IGNORED); if (transform.statusCode == TangoPoseData.POSE_VALID) { mRenderer.updatePointCloud(pointCloud, transform.matrix); } } // Update current camera pose. try { // Calculate the last depth camera pose. This transform is used to display // frustum in third and top down view, and used to render camera pose in // first person view. TangoPoseData lastFramePose = TangoSupport.getPoseAtTime(0, TangoPoseData.COORDINATE_FRAME_START_OF_SERVICE, TangoPoseData.COORDINATE_FRAME_DEVICE, TangoSupport.TANGO_SUPPORT_ENGINE_OPENGL, mDisplayRotation); mRenderer.updateCameraPose(lastFramePose); } catch (TangoErrorException e) { Log.e(TAG, "Could not get valid transform"); } } } @Override public boolean callPreFrame() { return true; } @Override public void onPreDraw(long sceneTime, double deltaTime) { } @Override public void onPostFrame(long sceneTime, double deltaTime) { } }); mSurfaceView.setSurfaceRenderer(mRenderer); } /** * Sets up TangoUX layout and sets its listener. */ private TangoUx setupTangoUxAndLayout() { TangoUxLayout uxLayout = (TangoUxLayout) findViewById(R.id.layout_tango); TangoUx tangoUx = new TangoUx(this); tangoUx.setLayout(uxLayout); tangoUx.setUxExceptionEventListener(mUxExceptionListener); return tangoUx; } /* * This is an advanced way of using UX exceptions. In most cases developers can just use the in * built exception notifications using the Ux Exception layout. In case a developer doesn't want * to use the default Ux Exception notifications, he can set the UxException listener as shown * below. * In this example we are just logging all the ux exceptions to logcat, but in a real app, * developers should use these exceptions to contextually notify the user and help direct the * user in using the device in a way Tango service expects it. */ private UxExceptionEventListener mUxExceptionListener = new UxExceptionEventListener() { @Override public void onUxExceptionEvent(UxExceptionEvent uxExceptionEvent) { if (uxExceptionEvent.getType() == UxExceptionEvent.TYPE_LYING_ON_SURFACE) { Log.i(TAG, "Device lying on surface "); } if (uxExceptionEvent.getType() == UxExceptionEvent.TYPE_FEW_DEPTH_POINTS) { Log.i(TAG, "Very few depth points in mPoint cloud "); } if (uxExceptionEvent.getType() == UxExceptionEvent.TYPE_FEW_FEATURES) { Log.i(TAG, "Invalid poses in MotionTracking "); } if (uxExceptionEvent.getType() == UxExceptionEvent.TYPE_INCOMPATIBLE_VM) { Log.i(TAG, "Device not running on ART"); } if (uxExceptionEvent.getType() == UxExceptionEvent.TYPE_MOTION_TRACK_INVALID) { Log.i(TAG, "Invalid poses in MotionTracking "); } if (uxExceptionEvent.getType() == UxExceptionEvent.TYPE_MOVING_TOO_FAST) { Log.i(TAG, "Invalid poses in MotionTracking "); } if (uxExceptionEvent.getType() == UxExceptionEvent.TYPE_FISHEYE_CAMERA_OVER_EXPOSED) { Log.i(TAG, "Fisheye Camera Over Exposed"); } if (uxExceptionEvent.getType() == UxExceptionEvent.TYPE_FISHEYE_CAMERA_UNDER_EXPOSED) { Log.i(TAG, "Fisheye Camera Under Exposed "); } if (uxExceptionEvent.getType() == UxExceptionEvent.TYPE_TANGO_SERVICE_NOT_RESPONDING) { Log.i(TAG, "TangoService is not responding "); } } }; /** * First Person button onClick callback. */ public void onFirstPersonClicked(View v) { mRenderer.setFirstPersonView(); } /** * Third Person button onClick callback. */ public void onThirdPersonClicked(View v) { mRenderer.setThirdPersonView(); } /** * Top-down button onClick callback. */ public void onTopDownClicked(View v) { mRenderer.setTopDownView(); } @Override public boolean onTouchEvent(MotionEvent event) { mRenderer.onTouchEvent(event); return true; } /** * Calculates the average depth from a point cloud buffer. * * @param pointCloudBuffer * @param numPoints * @return Average depth. */ private float getAveragedDepth(FloatBuffer pointCloudBuffer, int numPoints) { float totalZ = 0; float averageZ = 0; if (numPoints != 0) { int numFloats = 4 * numPoints; for (int i = 2; i < numFloats; i = i + 4) { totalZ = totalZ + pointCloudBuffer.get(i); } averageZ = totalZ / numPoints; } return averageZ; } private float[] getAveragedXY(FloatBuffer pointCloudBuffer, int numPoints) { float ret[] = new float[2]; // initialize ret[0] = 0; ret[1] = 0; float totalX = 0; float averageX = 0; float totalY = 0; float averageY = 0; if (numPoints != 0) { int numFloats = 4 * numPoints; for (int i = 1; i < numFloats; i = i + 4) { totalX = totalX + pointCloudBuffer.get(i - 1); totalY = totalY + pointCloudBuffer.get(i); } averageX = totalX / numPoints; averageY = totalY / numPoints; ret[0] = averageX; ret[1] = averageY; } return ret; } /** * Query the display's rotation. */ private void setDisplayRotation() { Display display = getWindowManager().getDefaultDisplay(); mDisplayRotation = display.getRotation(); } /** * Display toast on UI thread. * * @param resId The resource id of the string resource to use. Can be formatted text. */ private void showsToastAndFinishOnUiThread(final int resId) { runOnUiThread(new Runnable() { @Override public void run() { Toast.makeText(PointCloudActivity.this, getString(resId), Toast.LENGTH_LONG).show(); finish(); } }); } }