Ask for permission only when needed

TangoRosStreamer/app/src/main/java/eu/intermodalics/tango_ros_streamer/activities/RunningActivity.java

@@ -68,6 +68,7 @@
 import eu.intermodalics.tango_ros_common.Logger;
 import eu.intermodalics.tango_ros_common.MasterConnectionChecker;
 import eu.intermodalics.tango_ros_common.TangoServiceClientNode;
+import eu.intermodalics.tango_ros_common.TangoServiceServerNode;
 import eu.intermodalics.tango_ros_streamer.nodes.ImuNode;
 import eu.intermodalics.tango_ros_common.ParameterNode;
 import eu.intermodalics.tango_ros_streamer.R;
@@ -76,16 +77,17 @@
 import tango_ros_messages.TangoConnectResponse;
 
 public class RunningActivity extends AppCompatRosActivity implements
-        SaveMapDialog.CallbackListener, TangoServiceClientNode.CallbackListener {
+        SaveMapDialog.CallbackListener, TangoServiceClientNode.CallbackListener,
+        TangoServiceServerNode.CallbackListener {
     private static final String TAG = RunningActivity.class.getSimpleName();
     private static final String TAGS_TO_LOG = TAG + ", " + "tango_client_api, " + "Registrar, "
             + "DefaultPublisher, " + "native, " + "DefaultPublisher" ;
     private static final int LOG_TEXT_MAX_LENGTH = 5000;
     private static final int MAX_TANGO_CONNECTION_TRY = 50;
 
     private static final String REQUEST_TANGO_PERMISSION_ACTION = "android.intent.action.REQUEST_TANGO_PERMISSION";
-    public static final String EXTRA_KEY_PERMISSIONTYPE = "PERMISSIONTYPE";
-    public static final String EXTRA_VALUE_ADF = "ADF_LOAD_SAVE_PERMISSION";
+    private static final String EXTRA_KEY_PERMISSIONTYPE = "PERMISSIONTYPE";
+    private static final String EXTRA_VALUE_ADF = "ADF_LOAD_SAVE_PERMISSION";
     private static final String EXTRA_VALUE_DATASET = "DATASET_PERMISSION";
     private static final int REQUEST_CODE_ADF_PERMISSION = 111;
     private static final int REQUEST_CODE_DATASET_PERMISSION = 112;
@@ -117,17 +119,14 @@ enum TangoStatus {
     private CountDownLatch mRosConnectionLatch;
     private ParameterNode mParameterNode;
     private TangoServiceClientNode mTangoServiceClientNode;
+    private TangoServiceServerNode mTangoServiceServerNode;
     private ImuNode mImuNode;
     private RosStatus mRosStatus = RosStatus.UNKNOWN;
     private TangoStatus mTangoStatus = TangoStatus.UNKNOWN;
     private Logger mLogger;
     private boolean mCreateNewMap = false;
     private boolean mMapSaved = false;
     private HashMap<String, String> mUuidsNamesHashMap;
-    // True after the user answered the ADF permission popup (the permission has not been necessarily granted).
-    private boolean mAdfPermissionHasBeenAnswered = false;
-    // True after the user answered the dataset permission popup (the permission has not been necessarily granted).
-    private boolean mDatasetPermissionHasBeenAnswered = false;
 
     // UI objects.
     private Menu mToolbarMenu;
@@ -247,7 +246,8 @@ public void run() {
                     mTangoLightImageView.setImageDrawable(getResources().getDrawable(R.drawable.btn_radio_on_orange_light));
                 } else if (status == TangoStatus.SERVICE_CONNECTED) {
                     mTangoLightImageView.setImageDrawable(getResources().getDrawable(R.drawable.btn_radio_on_green_light));
-                } else {
+                } else if (status == TangoStatus.SERVICE_NOT_CONNECTED ||
+                           status == TangoStatus.NO_FIRST_VALID_POSE) {
                     mTangoLightImageView.setImageDrawable(getResources().getDrawable(R.drawable.btn_radio_on_red_light));
                 }
             }
@@ -432,6 +432,18 @@ private void saveUuidsNamestoHashMap() {
         mTangoServiceClientNode.callGetMapUuidsService();
     }
 
+    @Override
+    public void onRequestPermissionServiceCalled(tango_ros_messages.RequestPermissionRequest request,
+                                          tango_ros_messages.RequestPermissionResponse response) {
+        if (request.getPermission() == tango_ros_messages.RequestPermissionRequest.ADF_PERMISSION) {
+            getTangoPermission(EXTRA_VALUE_ADF, REQUEST_CODE_ADF_PERMISSION);
+        } else if (request.getPermission() == tango_ros_messages.RequestPermissionRequest.DATASET_PERMISSION) {
+            getTangoPermission(EXTRA_VALUE_DATASET, REQUEST_CODE_DATASET_PERMISSION);
+        } else {
+            Log.e(TAG, "Unknown permission requested: " + request.getPermission());
+        }
+    }
+
     private void getTangoPermission(String permissionType, int requestCode) {
         Intent intent = new Intent();
         intent.setAction(REQUEST_TANGO_PERMISSION_ACTION);
@@ -530,9 +542,8 @@ protected void onActivityResult(int requestCode, int resultCode, Intent data) {
                         getString(R.string.pref_log_file_default));
                 mLogger.setLogFileName(logFileName);
                 mLogger.start();
-                getTangoPermission(EXTRA_VALUE_ADF, REQUEST_CODE_ADF_PERMISSION);
-                getTangoPermission(EXTRA_VALUE_DATASET, REQUEST_CODE_DATASET_PERMISSION);
                 updateSaveMapButton();
+                initAndStartRosJavaNode();
             } else if (requestCode == StartSettingsActivityRequest.STANDARD_RUN) {
                 // It is ok to change the log file name at runtime.
                 String logFileName = mSharedPref.getString(getString(R.string.pref_log_file_key),
@@ -550,22 +561,10 @@ protected void onActivityResult(int requestCode, int resultCode, Intent data) {
             if (resultCode == RESULT_CANCELED) {
                 // No Tango permissions granted by the user.
                 displayToastMessage(R.string.tango_permission_denied);
+                mTangoServiceServerNode.onRequestPermissionAnswered(false);
+            } else {
+                mTangoServiceServerNode.onRequestPermissionAnswered(true);
             }
-            if (requestCode == REQUEST_CODE_ADF_PERMISSION) {
-                // The user answered the ADF permission popup (the permission has not been necessarily granted).
-                mAdfPermissionHasBeenAnswered = true;
-            }
-            if (requestCode ==  REQUEST_CODE_DATASET_PERMISSION) {
-                // The user answered the dataset permission popup (the permission has not been necessarily granted).
-                mDatasetPermissionHasBeenAnswered = true;
-            }
-            if (mAdfPermissionHasBeenAnswered && mDatasetPermissionHasBeenAnswered) {
-                // Both ADF and dataset permissions popup have been answered by the user, the node
-                // can start.
-                Log.i(TAG, "initAndStartRosJavaNode");
-                initAndStartRosJavaNode();
-            }
-
         }
     }
 
@@ -631,6 +630,11 @@ protected void init(NodeMainExecutor nodeMainExecutor) {
         mTangoServiceClientNode = new TangoServiceClientNode(this);
         nodeConfiguration.setNodeName(mTangoServiceClientNode.getDefaultNodeName());
         nodeMainExecutor.execute(mTangoServiceClientNode, nodeConfiguration);
+        // ServiceServer node which is responsible for providing ros services.
+        mTangoServiceServerNode = new TangoServiceServerNode();
+        mTangoServiceServerNode.setCallbackListener(this);
+        nodeConfiguration.setNodeName(mTangoServiceServerNode.getDefaultNodeName());
+        nodeMainExecutor.execute(mTangoServiceServerNode, nodeConfiguration);
         // Create node publishing IMU data.
         mImuNode = new ImuNode(this);
         nodeConfiguration.setNodeName(mImuNode.getDefaultNodeName());
@@ -688,8 +692,7 @@ public void startMasterChooser() {
         boolean appPreviouslyStarted = mSharedPref.getBoolean(getString(R.string.pref_previously_started_key), false);
         if (appPreviouslyStarted) {
             mLogger.start();
-            getTangoPermission(EXTRA_VALUE_ADF, REQUEST_CODE_ADF_PERMISSION);
-            getTangoPermission(EXTRA_VALUE_DATASET, REQUEST_CODE_DATASET_PERMISSION);
+            initAndStartRosJavaNode();
         } else {
             Intent intent = new Intent(this, SettingsActivity.class);
             startActivityForResult(intent, StartSettingsActivityRequest.FIRST_RUN);
@@ -700,6 +703,7 @@ public void startMasterChooser() {
      * This function initializes the tango ros node with RosJava interface.
      */
     private void initAndStartRosJavaNode() {
+        Log.i(TAG, "initAndStartRosJavaNode");
         this.nodeMainExecutorService.addListener(new NodeMainExecutorServiceListener() {
             @Override
             public void onShutdown(NodeMainExecutorService nodeMainExecutorService) {

tango_ros_common/tango_ros_common/src/main/java/eu/intermodalics/tango_ros_common/ParameterNode.java

@@ -72,6 +72,33 @@ public void syncLocalPreferencesWithParameterServer() {
         uploadPreferencesToParameterServer();
     }
 
+    public Boolean getBoolParam(String paramName) {
+        Boolean booleanValue = mConnectedNode.getParameterTree().getBoolean(NodeNamespaceHelper.BuildTangoRosNodeNamespaceName(paramName), false);
+        return booleanValue;
+    }
+
+    public Integer getIntParam(String paramName) {
+        Integer intValue = mConnectedNode.getParameterTree().getInteger(NodeNamespaceHelper.BuildTangoRosNodeNamespaceName(paramName), 0);
+        return intValue;
+    }
+
+    public String getStringParam(String paramName) {
+        String stringValue = mConnectedNode.getParameterTree().getString(NodeNamespaceHelper.BuildTangoRosNodeNamespaceName(paramName), "");
+        return stringValue;
+    }
+
+    public void setBoolParam(String paramName, Boolean booleanValue) {
+        mConnectedNode.getParameterTree().set(NodeNamespaceHelper.BuildTangoRosNodeNamespaceName(paramName), booleanValue);
+    }
+
+    public void setIntParam(String paramName, Integer intValue) {
+        mConnectedNode.getParameterTree().set(NodeNamespaceHelper.BuildTangoRosNodeNamespaceName(paramName), intValue);
+    }
+
+    public void setStringParam(String paramName, String stringValue) {
+        mConnectedNode.getParameterTree().set(NodeNamespaceHelper.BuildTangoRosNodeNamespaceName(paramName), stringValue);
+    }
+
     // Set ROS params according to preferences.
     public void uploadPreferencesToParameterServer() {
         mLog.info("Upload preferences to parameter server.");
@@ -89,13 +116,13 @@ public void uploadPreferenceToParameterServer(String paramName) {
         String valueType = mParamNames.get(paramName);
         if (valueType == "boolean") {
             Boolean booleanValue = mSharedPreferences.getBoolean(paramName, false);
-            mConnectedNode.getParameterTree().set(NodeNamespaceHelper.BuildTangoRosNodeNamespaceName(paramName), booleanValue);
+            setBoolParam(paramName, booleanValue);
         } else if (valueType == "int_as_string") {
             String stringValue = mSharedPreferences.getString(paramName, "0");
-            mConnectedNode.getParameterTree().set(NodeNamespaceHelper.BuildTangoRosNodeNamespaceName(paramName), Integer.parseInt(stringValue));
+            setIntParam(paramName, Integer.parseInt(stringValue));
         } else if (valueType == "string") {
             String stringValue = mSharedPreferences.getString(paramName, "");
-            mConnectedNode.getParameterTree().set(NodeNamespaceHelper.BuildTangoRosNodeNamespaceName(paramName), stringValue);
+            setStringParam(paramName, stringValue);
         }
     }
 
@@ -110,15 +137,15 @@ public void setPreferencesFromParameterServer() {
         for (String paramName : mParamNames.keySet()) {
             if (mConnectedNode.getParameterTree().has(NodeNamespaceHelper.BuildTangoRosNodeNamespaceName(paramName))) {
                 if (mParamNames.get(paramName) == "boolean") {
-                    Boolean booleanValue = mConnectedNode.getParameterTree().getBoolean(NodeNamespaceHelper.BuildTangoRosNodeNamespaceName(paramName), false);
+                    Boolean booleanValue = getBoolParam(paramName);
                     editor.putBoolean(paramName, booleanValue);
                 }
                 if (mParamNames.get(paramName) == "int_as_string") {
-                    Integer intValue = mConnectedNode.getParameterTree().getInteger(NodeNamespaceHelper.BuildTangoRosNodeNamespaceName(paramName), 0);
+                    Integer intValue = getIntParam(paramName);
                     editor.putString(paramName, intValue.toString());
                 }
                 if (mParamNames.get(paramName) == "string") {
-                    String stringValue = mConnectedNode.getParameterTree().getString(NodeNamespaceHelper.BuildTangoRosNodeNamespaceName(paramName), "");
+                    String stringValue = getStringParam(paramName);
                     editor.putString(paramName, stringValue);
                 }
             }

tango_ros_common/tango_ros_common/src/main/java/eu/intermodalics/tango_ros_common/TangoServiceClientNode.java

@@ -81,8 +81,8 @@ public void onStart(ConnectedNode connectedNode) {
         mConnectedNode = connectedNode;
         mLog = connectedNode.getLog();
 
-        Subscriber<Int8> subscriber = mConnectedNode.newSubscriber(NodeNamespaceHelper.BuildTangoRosNodeNamespaceName(TANGO_STATUS_TOPIC_NAME), Int8._TYPE);
-        subscriber.addMessageListener(new MessageListener<Int8>() {
+        Subscriber<Int8> statusSubscriber = mConnectedNode.newSubscriber(NodeNamespaceHelper.BuildTangoRosNodeNamespaceName(TANGO_STATUS_TOPIC_NAME), Int8._TYPE);
+        statusSubscriber.addMessageListener(new MessageListener<Int8>() {
             @Override
             public void onNewMessage(Int8 status) {
                 mCallbackListener.onTangoStatus(status.getData());

tango_ros_common/tango_ros_common/src/main/java/eu/intermodalics/tango_ros_common/TangoServiceServerNode.java

@@ -0,0 +1,96 @@
+/*
+ * Copyright 2016 Intermodalics 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 eu.intermodalics.tango_ros_common;
+
+import android.os.SystemClock;
+
+import org.apache.commons.logging.Log;
+import org.ros.namespace.GraphName;
+import org.ros.node.AbstractNodeMain;
+import org.ros.node.ConnectedNode;
+import org.ros.node.service.ServiceResponseBuilder;
+import org.ros.node.service.ServiceServer;
+
+import java.util.concurrent.atomic.AtomicBoolean;
+
+/**
+ * Rosjava node that implements a server for tango ROS services.
+ */
+public class TangoServiceServerNode extends AbstractNodeMain {
+    private static final String TAG = TangoServiceServerNode.class.getSimpleName();
+    private static final String NODE_NAME = "tango_service_server_node";
+    private static final String REQUEST_PERMISSION_SRV_NAME = "request_permission";
+
+    ConnectedNode mConnectedNode;
+    private Log mLog;
+    CallbackListener mCallbackListener = new DefaultCallbackListener();
+
+    AtomicBoolean mRequestPermissionAnswered = new AtomicBoolean(false);
+    AtomicBoolean mRequestPermissionGranted = new AtomicBoolean(false);
+
+    public interface CallbackListener {
+        void onRequestPermissionServiceCalled(tango_ros_messages.RequestPermissionRequest request,
+                                              tango_ros_messages.RequestPermissionResponse response);
+    }
+
+    public class DefaultCallbackListener implements CallbackListener{
+
+        public DefaultCallbackListener() {}
+
+        @Override
+        public void onRequestPermissionServiceCalled(tango_ros_messages.RequestPermissionRequest request,
+                                                     tango_ros_messages.RequestPermissionResponse response) {}
+    }
+
+    public TangoServiceServerNode() {}
+
+    public void setCallbackListener(CallbackListener callbackListener) {
+        mCallbackListener = callbackListener;
+    }
+
+    @Override
+    public GraphName getDefaultNodeName() {
+        return GraphName.of(NODE_NAME);
+    }
+
+    public void onStart(ConnectedNode connectedNode) {
+        mConnectedNode = connectedNode;
+        mLog = connectedNode.getLog();
+
+        ServiceServer<tango_ros_messages.RequestPermissionRequest, tango_ros_messages.RequestPermissionResponse> server =
+                mConnectedNode.newServiceServer(NodeNamespaceHelper.BuildTangoRosNodeNamespaceName(REQUEST_PERMISSION_SRV_NAME),
+                        tango_ros_messages.RequestPermission._TYPE,
+                        new ServiceResponseBuilder<tango_ros_messages.RequestPermissionRequest, tango_ros_messages.RequestPermissionResponse>() {
+                    @Override
+                    public void build(tango_ros_messages.RequestPermissionRequest request, tango_ros_messages.RequestPermissionResponse response) {
+                        mRequestPermissionAnswered.set(false);
+                        mRequestPermissionGranted.set(false);
+                        mCallbackListener.onRequestPermissionServiceCalled(request, response);
+                        while (!mRequestPermissionAnswered.get()) {
+                            mLog.info("Waiting for user to answer permission request.");
+                            SystemClock.sleep(100);
+                        }
+                        response.setGranted(mRequestPermissionGranted.get());
+                    }
+                });
+    }
+
+    public void onRequestPermissionAnswered(boolean granted) {
+        mRequestPermissionGranted.set(granted);
+        mRequestPermissionAnswered.set(true);
+    }
+}

tango_ros_common/tango_ros_messages/CMakeLists.txt

@@ -11,6 +11,7 @@ add_service_files(
   FILES
   GetMapName.srv
   GetMapUuids.srv
+  RequestPermission.srv
   SaveMap.srv
   TangoConnect.srv
 )
@@ -39,6 +40,9 @@ set(file_names_to_copy
   GetMapUuids
   GetMapUuidsRequest
   GetMapUuidsResponse
+  RequestPermission
+  RequestPermissionRequest
+  RequestPermissionResponse
   SaveMap
   SaveMapRequest
   SaveMapResponse

tango_ros_common/tango_ros_messages/srv/RequestPermission.srv

@@ -0,0 +1,6 @@
+int8 ADF_PERMISSION=0
+int8 DATASET_PERMISSION=1
+int8 permission
+---
+bool granted
+

tango_ros_common/tango_ros_native/include/tango_ros_native/tango_ros_node.h

@@ -40,6 +40,7 @@
 #include <sensor_msgs/PointCloud2.h>
 #include <tango_ros_messages/GetMapName.h>
 #include <tango_ros_messages/GetMapUuids.h>
+#include <tango_ros_messages/RequestPermission.h>
 #include <tango_ros_messages/SaveMap.h>
 #include <tango_ros_messages/TangoConnect.h>
 #include <tf/transform_broadcaster.h>
@@ -85,8 +86,9 @@ const std::string GET_MAP_NAME_SERVICE_NAME = "get_map_name";
 const std::string GET_MAP_UUIDS_SERVICE_NAME = "get_map_uuids";
 const std::string SAVE_MAP_SERVICE_NAME = "save_map";
 const std::string CONNECT_SERVICE_NAME = "connect";
+const std::string REQUEST_PERMISSION_SERVICE_NAME = "request_permission";
 
-const std::string DATASETS_PATH = "/sdcard/tango_ros_streamer/datasets/";
+const std::string DEFAULT_DATASETS_PATH = "/sdcard/tango_ros_streamer/datasets/";
 
 // Localization mode values.
 // See http://developers.google.com/tango/overview/area-learning to know more
@@ -192,6 +194,10 @@ class TangoRosNode : public ::nodelet::Nodelet {
   bool TangoConnectServiceCallback(
           const tango_ros_messages::TangoConnect::Request &request,
           tango_ros_messages::TangoConnect::Response& response);
+  // Request ADF permision via ros service call.
+  void RequestADFPermission();
+  // Request dataset permision via ros service call.
+  void RequestDatasetPermission();
 
   TangoConfig tango_config_;
   ros::NodeHandle node_handle_;
@@ -270,6 +276,7 @@ class TangoRosNode : public ::nodelet::Nodelet {
   ros::ServiceServer get_map_uuids_service_;
   ros::ServiceServer save_map_service_;
   ros::ServiceServer tango_connect_service_;
+  ros::ServiceClient request_permission_service_;
 };
 }  // namespace tango_ros_native
 #endif  // TANGO_ROS_NODE_H_