diff --git a/TangoRosStreamer/app/src/main/java/eu/intermodalics/tango_ros_streamer/activities/RunningActivity.java b/TangoRosStreamer/app/src/main/java/eu/intermodalics/tango_ros_streamer/activities/RunningActivity.java index 361051d..07b6f45 100644 --- a/TangoRosStreamer/app/src/main/java/eu/intermodalics/tango_ros_streamer/activities/RunningActivity.java +++ b/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,7 +77,8 @@ 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" ; @@ -84,8 +86,8 @@ public class RunningActivity extends AppCompatRosActivity implements 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,6 +119,7 @@ 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; @@ -124,10 +127,6 @@ enum TangoStatus { private boolean mCreateNewMap = false; private boolean mMapSaved = false; private HashMap 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) { diff --git a/tango_ros_common/tango_ros_common/src/main/java/eu/intermodalics/tango_ros_common/ParameterNode.java b/tango_ros_common/tango_ros_common/src/main/java/eu/intermodalics/tango_ros_common/ParameterNode.java index 7be1ee7..8a38d4e 100644 --- a/tango_ros_common/tango_ros_common/src/main/java/eu/intermodalics/tango_ros_common/ParameterNode.java +++ b/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); } } diff --git a/tango_ros_common/tango_ros_common/src/main/java/eu/intermodalics/tango_ros_common/TangoServiceClientNode.java b/tango_ros_common/tango_ros_common/src/main/java/eu/intermodalics/tango_ros_common/TangoServiceClientNode.java index a6f4a92..a3cdd2e 100644 --- a/tango_ros_common/tango_ros_common/src/main/java/eu/intermodalics/tango_ros_common/TangoServiceClientNode.java +++ b/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 subscriber = mConnectedNode.newSubscriber(NodeNamespaceHelper.BuildTangoRosNodeNamespaceName(TANGO_STATUS_TOPIC_NAME), Int8._TYPE); - subscriber.addMessageListener(new MessageListener() { + Subscriber statusSubscriber = mConnectedNode.newSubscriber(NodeNamespaceHelper.BuildTangoRosNodeNamespaceName(TANGO_STATUS_TOPIC_NAME), Int8._TYPE); + statusSubscriber.addMessageListener(new MessageListener() { @Override public void onNewMessage(Int8 status) { mCallbackListener.onTangoStatus(status.getData()); diff --git a/tango_ros_common/tango_ros_common/src/main/java/eu/intermodalics/tango_ros_common/TangoServiceServerNode.java b/tango_ros_common/tango_ros_common/src/main/java/eu/intermodalics/tango_ros_common/TangoServiceServerNode.java new file mode 100644 index 0000000..fd8ca53 --- /dev/null +++ b/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 server = + mConnectedNode.newServiceServer(NodeNamespaceHelper.BuildTangoRosNodeNamespaceName(REQUEST_PERMISSION_SRV_NAME), + tango_ros_messages.RequestPermission._TYPE, + new ServiceResponseBuilder() { + @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); + } +} \ No newline at end of file diff --git a/tango_ros_common/tango_ros_messages/CMakeLists.txt b/tango_ros_common/tango_ros_messages/CMakeLists.txt index 0116ce7..d839b84 100644 --- a/tango_ros_common/tango_ros_messages/CMakeLists.txt +++ b/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 diff --git a/tango_ros_common/tango_ros_messages/srv/RequestPermission.srv b/tango_ros_common/tango_ros_messages/srv/RequestPermission.srv new file mode 100644 index 0000000..fe5bcf1 --- /dev/null +++ b/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 + diff --git a/tango_ros_common/tango_ros_native/include/tango_ros_native/tango_ros_node.h b/tango_ros_common/tango_ros_native/include/tango_ros_native/tango_ros_node.h index 895bb15..f58e6cd 100644 --- a/tango_ros_common/tango_ros_native/include/tango_ros_native/tango_ros_node.h +++ b/tango_ros_common/tango_ros_native/include/tango_ros_native/tango_ros_node.h @@ -40,6 +40,7 @@ #include #include #include +#include #include #include #include @@ -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_ diff --git a/tango_ros_common/tango_ros_native/src/tango_ros_node.cpp b/tango_ros_common/tango_ros_native/src/tango_ros_node.cpp index a8ae365..77efaab 100644 --- a/tango_ros_common/tango_ros_native/src/tango_ros_node.cpp +++ b/tango_ros_common/tango_ros_native/src/tango_ros_node.cpp @@ -212,6 +212,8 @@ void TangoRosNode::onInit() { CONNECT_SERVICE_NAME, boost::bind( &TangoRosNode::TangoConnectServiceCallback, this, _1, _2)); + request_permission_service_ = node_handle_.serviceClient(REQUEST_PERMISSION_SERVICE_NAME); + tango_status_ = TangoStatus::UNKNOWN; if (!node_handle_.hasParam(CREATE_NEW_MAP_PARAM_NAME)) { @@ -224,7 +226,7 @@ void TangoRosNode::onInit() { node_handle_.setParam(LOCALIZATION_MAP_UUID_PARAM_NAME, ""); } if (!node_handle_.hasParam(DATASET_PATH_PARAM_NAME)) { - node_handle_.setParam(DATASET_PATH_PARAM_NAME, ""); + node_handle_.setParam(DATASET_PATH_PARAM_NAME, DEFAULT_DATASETS_PATH); } if (!node_handle_.hasParam(DATASET_UUID_PARAM_NAME)) { node_handle_.setParam(DATASET_UUID_PARAM_NAME, ""); @@ -291,6 +293,34 @@ TangoErrorType TangoRosNode::OnTangoServiceConnected() { return TANGO_SUCCESS; } +void TangoRosNode::RequestADFPermission() { + tango_ros_messages::RequestPermission srv; + srv.request.permission = tango_ros_messages::RequestPermission::Request::ADF_PERMISSION; + if (request_permission_service_.call(srv)) { + if (srv.response.granted) { + LOG(INFO) << "ADF permission has been requested and granted by the user."; + } else { + LOG(WARNING) << "ADF permission has been requested but not granted by the user."; + } + } else { + LOG(ERROR) << "Failed to call service to request ADF permission"; + } +} + +void TangoRosNode::RequestDatasetPermission() { + tango_ros_messages::RequestPermission srv; + srv.request.permission = tango_ros_messages::RequestPermission::Request::DATASET_PERMISSION; + if (request_permission_service_.call(srv)) { + if (srv.response.granted) { + LOG(INFO) << "Dataset permission has been requested and granted by the user."; + } else { + LOG(WARNING) << "Dataset permission has been requested but not granted by the user."; + } + } else { + LOG(ERROR) << "Failed to call service to request dataset permission"; + } +} + TangoErrorType TangoRosNode::TangoSetupConfig() { const char* function_name = "TangoRosNode::TangoSetupConfig()"; @@ -375,7 +405,7 @@ TangoErrorType TangoRosNode::TangoSetupConfig() { return result; } std::string datasets_path; - node_handle_.param(DATASET_PATH_PARAM_NAME, datasets_path, DATASETS_PATH); + node_handle_.param(DATASET_PATH_PARAM_NAME, datasets_path, DEFAULT_DATASETS_PATH); const char* config_datasets_path = "config_datasets_path"; result = TangoConfig_setString(tango_config_, config_datasets_path, datasets_path.c_str()); if (result != TANGO_SUCCESS) { @@ -385,12 +415,15 @@ TangoErrorType TangoRosNode::TangoSetupConfig() { } std::string dataset_uuid; node_handle_.param(DATASET_UUID_PARAM_NAME, dataset_uuid, std::string("")); - const char* config_experimental_load_dataset_UUID = "config_experimental_load_dataset_UUID"; - result = TangoConfig_setString(tango_config_, config_experimental_load_dataset_UUID, dataset_uuid.c_str()); - if (result != TANGO_SUCCESS) { - LOG(ERROR) << function_name << ", TangoConfig_setString " - << config_experimental_load_dataset_UUID << " error: " << result; - return result; + if (dataset_uuid != "") { + RequestDatasetPermission(); + const char* config_experimental_load_dataset_UUID = "config_experimental_load_dataset_UUID"; + result = TangoConfig_setString(tango_config_, config_experimental_load_dataset_UUID, dataset_uuid.c_str()); + if (result != TANGO_SUCCESS) { + LOG(ERROR) << function_name << ", TangoConfig_setString " + << config_experimental_load_dataset_UUID << " error: " << result; + return result; + } } if (point_cloud_manager_ == nullptr) { int32_t max_point_cloud_elements; @@ -544,7 +577,6 @@ TangoErrorType TangoRosNode::ConnectToTangoAndSetUpNode() { return success; } - void TangoRosNode::TangoDisconnect() { StopPublishing(); if (tango_config_ != nullptr) { @@ -1102,6 +1134,7 @@ bool TangoRosNode::GetMapUuids( bool TangoRosNode::SaveMap(tango_ros_messages::SaveMap::Request &req, tango_ros_messages::SaveMap::Response &res) { + RequestADFPermission(); TangoErrorType result; TangoUUID map_uuid; result = TangoService_saveAreaDescription(&map_uuid); @@ -1154,6 +1187,7 @@ bool TangoRosNode::SaveMap(tango_ros_messages::SaveMap::Request &req, } std::string TangoRosNode::GetAvailableMapUuidsList() { + RequestADFPermission(); char* uuid_list; TangoErrorType result = TangoService_getAreaDescriptionUUIDList(&uuid_list); if (result != TANGO_SUCCESS) { @@ -1170,6 +1204,7 @@ std::string TangoRosNode::GetAvailableMapUuidsList() { } std::string TangoRosNode::GetMapNameFromUuid(const std::string& map_uuid) { + RequestADFPermission(); size_t size = 0; char* value; TangoAreaDescriptionMetadata metadata;