diff --git a/app/android/AndroidManifest.xml.in b/app/android/AndroidManifest.xml.in index d03c6189..db61112f 100644 --- a/app/android/AndroidManifest.xml.in +++ b/app/android/AndroidManifest.xml.in @@ -2,7 +2,7 @@ diff --git a/app/android/jni/CameraTango.cpp b/app/android/jni/CameraTango.cpp index 4bdbbd77..50a6bb6b 100644 --- a/app/android/jni/CameraTango.cpp +++ b/app/android/jni/CameraTango.cpp @@ -505,7 +505,7 @@ static rtabmap::Transform opticalRotation( 0.0f, 0.0f, -1.0f, 0.0f); void CameraTango::poseReceived(const Transform & pose) { - if(!pose.isNull() && pose.getNormSquared() < 100000) + if(!pose.isNull()) { // send pose of the camera (without optical rotation), not the device Transform p = pose*deviceTColorCamera_*opticalRotation; diff --git a/app/android/jni/RTABMapApp.cpp b/app/android/jni/RTABMapApp.cpp index 1291ccbe..9d7ae701 100644 --- a/app/android/jni/RTABMapApp.cpp +++ b/app/android/jni/RTABMapApp.cpp @@ -184,6 +184,7 @@ RTABMapApp::RTABMapApp() : lastDrawnCloudsCount_(0), renderingTime_(0.0f), lastPostRenderEventTime_(0.0), + lastPoseEventTime_(0.0), visualizingMesh_(false), exportedMeshUpdated_(false), optMesh_(new pcl::TextureMesh), @@ -254,6 +255,7 @@ void RTABMapApp::onCreate(JNIEnv* env, jobject caller_activity) lastDrawnCloudsCount_ = 0; renderingTime_ = 0.0f; lastPostRenderEventTime_ = 0.0; + lastPoseEventTime_ = 0.0; bufferedStatsData_.clear(); progressionStatus_.setJavaObjects(jvm, RTABMapActivity); main_scene_.setBackgroundColor(backgroundColor_, backgroundColor_, backgroundColor_); @@ -1041,6 +1043,7 @@ int RTABMapApp::Render() notifyCameraStarted = true; cameraJustInitialized_ = false; } + lastPoseEventTime_ = UTimer::now(); } rtabmap::OdometryEvent odomEvent; @@ -1249,6 +1252,7 @@ int RTABMapApp::Render() lastDrawnCloudsCount_ = 0; renderingTime_ = 0.0f; lastPostRenderEventTime_ = 0.0; + lastPoseEventTime_ = 0.0; bufferedStatsData_.clear(); } @@ -1652,6 +1656,12 @@ int RTABMapApp::Render() rtabmapEvents.clear(); lastPostRenderEventTime_ = UTimer::now(); + + if(lastPoseEventTime_>0.0 && UTimer::now()-lastPoseEventTime_ > 1.0) + { + UERROR("TangoPoseEventNotReceived"); + UEventsManager::post(new rtabmap::CameraTangoEvent(10, "TangoPoseEventNotReceived", uNumber2Str(UTimer::now()-lastPoseEventTime_))); + } } } diff --git a/app/android/jni/RTABMapApp.h b/app/android/jni/RTABMapApp.h index 0e0f6e49..b9649546 100644 --- a/app/android/jni/RTABMapApp.h +++ b/app/android/jni/RTABMapApp.h @@ -228,6 +228,7 @@ class RTABMapApp : public UEventsHandler { int lastDrawnCloudsCount_; float renderingTime_; double lastPostRenderEventTime_; + double lastPoseEventTime_; std::map bufferedStatsData_; bool visualizingMesh_; diff --git a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java b/app/android/src/com/introlab/rtabmap/RTABMapActivity.java index 2d11d09c..1607e8f4 100644 --- a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java +++ b/app/android/src/com/introlab/rtabmap/RTABMapActivity.java @@ -1385,9 +1385,13 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem { str = String.format("Too close! Tip: Scan from at least ~1 meter from surfaces.", value); } + else if(key.equals("TangoPoseEventNotReceived")) + { + str = String.format("No valid tango pose event received since %s sec.", value); + } else { - str = String.format("Unknown Tango event detected!? (type=%d)", type); + str = String.format("Unknown Tango event detected!? (type=%d, key=%s, value=%s)", type, key, value); } if(str!=null) {