00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 package eu.intermodalics.tango_ros_streamer.activities;
00018
00019 import android.animation.AnimatorInflater;
00020 import android.animation.AnimatorSet;
00021 import android.app.DialogFragment;
00022 import android.app.FragmentManager;
00023 import android.content.Intent;
00024 import android.content.ServiceConnection;
00025 import android.content.SharedPreferences;
00026 import android.net.Uri;
00027 import android.content.res.Configuration;
00028 import android.net.wifi.WifiManager;
00029 import android.os.AsyncTask;
00030 import android.os.Bundle;
00031 import android.os.StrictMode;
00032 import android.preference.PreferenceManager;
00033 import android.support.design.widget.Snackbar;
00034 import android.support.v7.widget.Toolbar;
00035 import android.text.format.Formatter;
00036 import android.text.method.ScrollingMovementMethod;
00037 import android.util.Log;
00038 import android.view.Menu;
00039 import android.view.MenuInflater;
00040 import android.view.MenuItem;
00041 import android.view.View;
00042 import android.widget.Button;
00043 import android.widget.CompoundButton;
00044 import android.widget.Switch;
00045 import android.widget.ImageView;
00046 import android.widget.TextView;
00047 import android.widget.Toast;
00048
00049 import org.apache.commons.io.FilenameUtils;
00050 import org.ros.address.InetAddressFactory;
00051 import org.ros.android.NodeMainExecutorService;
00052 import org.ros.android.NodeMainExecutorServiceListener;
00053 import org.ros.exception.RosRuntimeException;
00054 import org.ros.node.ConnectedNode;
00055 import org.ros.node.DefaultNodeListener;
00056 import org.ros.node.NodeConfiguration;
00057 import org.ros.node.NodeListener;
00058 import org.ros.node.NodeMainExecutor;
00059
00060 import java.io.File;
00061 import java.net.URI;
00062 import java.util.ArrayList;
00063 import java.util.HashMap;
00064 import java.util.List;
00065 import java.util.concurrent.CountDownLatch;
00066
00067 import eu.intermodalics.nodelet_manager.TangoNodeletManager;
00068 import eu.intermodalics.nodelet_manager.TangoInitializationHelper;
00069 import eu.intermodalics.nodelet_manager.TangoInitializationHelper.DefaultTangoServiceConnection;
00070
00071 import eu.intermodalics.tango_ros_common.Logger;
00072 import eu.intermodalics.tango_ros_common.MasterConnectionChecker;
00073 import eu.intermodalics.tango_ros_common.TangoServiceClientNode;
00074 import eu.intermodalics.tango_ros_streamer.android.LoadOccupancyGridDialog;
00075 import eu.intermodalics.tango_ros_streamer.nodes.ImuNode;
00076 import eu.intermodalics.tango_ros_common.ParameterNode;
00077 import eu.intermodalics.tango_ros_streamer.R;
00078 import eu.intermodalics.tango_ros_streamer.android.SaveMapDialog;
00079 import tango_ros_messages.TangoConnectRequest;
00080 import tango_ros_messages.TangoConnectResponse;
00081
00082 public class RunningActivity extends AppCompatRosActivity implements
00083 SaveMapDialog.CallbackListener, LoadOccupancyGridDialog.CallbackListener,
00084 TangoServiceClientNode.CallbackListener {
00085 private static final String TAG = RunningActivity.class.getSimpleName();
00086 private static final String TAGS_TO_LOG = TAG + ", " + "tango_client_api, " + "Registrar, "
00087 + "DefaultPublisher, " + "native, " + "DefaultPublisher" ;
00088 private static final int LOG_TEXT_MAX_LENGTH = 5000;
00089 private static final int MAX_TANGO_CONNECTION_TRY = 50;
00090
00091 private static final String REQUEST_TANGO_PERMISSION_ACTION = "android.intent.action.REQUEST_TANGO_PERMISSION";
00092 public static final String EXTRA_KEY_PERMISSIONTYPE = "PERMISSIONTYPE";
00093 public static final String EXTRA_VALUE_ADF = "ADF_LOAD_SAVE_PERMISSION";
00094 private static final String EXTRA_VALUE_DATASET = "DATASET_PERMISSION";
00095 private static final int REQUEST_CODE_ADF_PERMISSION = 111;
00096 private static final int REQUEST_CODE_DATASET_PERMISSION = 112;
00097 public static final String RESTART_TANGO = "restart_tango";
00098
00099 public static class StartSettingsActivityRequest {
00100 public static final int FIRST_RUN = 11;
00101 public static final int STANDARD_RUN = 12;
00102 }
00103
00104 enum RosStatus {
00105 UNKNOWN,
00106 MASTER_NOT_CONNECTED,
00107 MASTER_CONNECTED
00108 }
00109
00110
00111 enum TangoStatus {
00112 UNKNOWN,
00113 SERVICE_NOT_CONNECTED,
00114 NO_FIRST_VALID_POSE,
00115 SERVICE_CONNECTED
00116 }
00117
00118 private SharedPreferences mSharedPref;
00119 private TangoNodeletManager mTangoNodeletManager;
00120 private boolean mRunLocalMaster = false;
00121 private String mMasterUri = "";
00122 private CountDownLatch mRosConnectionLatch;
00123 private ParameterNode mParameterNode;
00124 private TangoServiceClientNode mTangoServiceClientNode;
00125 private ImuNode mImuNode;
00126 private RosStatus mRosStatus = RosStatus.UNKNOWN;
00127 private TangoStatus mTangoStatus = TangoStatus.UNKNOWN;
00128 private Logger mLogger;
00129 private boolean mCreateNewMap = false;
00130 private boolean mMapSaved = false;
00131 private HashMap<String, String> mUuidsNamesHashMap;
00132
00133 private boolean mAdfPermissionHasBeenAnswered = false;
00134
00135 private boolean mDatasetPermissionHasBeenAnswered = false;
00136 private ArrayList<String> mOccupancyGridNameList = new ArrayList<String>();
00137
00138
00139 private Menu mToolbarMenu;
00140 private TextView mUriTextView;
00141 private ImageView mRosLightImageView;
00142 private ImageView mTangoLightImageView;
00143 private Switch mlogSwitch;
00144 private boolean mDisplayLog = false;
00145 private TextView mLogTextView;
00146 private Button mSaveMapButton;
00147 private Button mLoadOccupancyGridButton;
00148 private Snackbar mSnackbarLoadNewMap;
00149 private Snackbar mSnackbarRosReconnection;
00150
00151 public RunningActivity() {
00152 super("TangoRosStreamer", "TangoRosStreamer");
00153 }
00154
00155 protected RunningActivity(String notificationTicker, String notificationTitle) {
00156 super(notificationTicker, notificationTitle);
00157 }
00158
00162 ServiceConnection mTangoServiceConnection = new DefaultTangoServiceConnection(
00163 new DefaultTangoServiceConnection.AfterConnectionCallback() {
00164 @Override
00165 public void execute() {
00166 if (TangoInitializationHelper.isTangoServiceBound()) {
00167 if (TangoInitializationHelper.isTangoVersionOk()) {
00168 Log.i(TAG, "Version of Tango is ok.");
00169 } else {
00170 updateTangoStatus(TangoStatus.SERVICE_NOT_CONNECTED);
00171 Log.e(TAG, getResources().getString(R.string.tango_version_error));
00172 displayToastMessage(R.string.tango_version_error);
00173 }
00174 } else {
00175 updateTangoStatus(TangoStatus.SERVICE_NOT_CONNECTED);
00176 Log.e(TAG, getString(R.string.tango_bind_error));
00177 displayToastMessage(R.string.tango_bind_error);
00178 }
00179 }
00180 }
00181 );
00182
00183 private void updateRosStatus(RosStatus status) {
00184 if (mRosStatus != status) {
00185 mRosStatus = status;
00186 }
00187 switchRosLight(status);
00188 SharedPreferences.Editor editor = mSharedPref.edit();
00189 editor.putInt(getString(R.string.ros_status), status.ordinal());
00190 editor.commit();
00191 }
00192
00193 private void switchRosLight(final RosStatus status) {
00194 runOnUiThread(new Runnable() {
00195 @Override
00196 public void run() {
00197 if (status == RosStatus.UNKNOWN) {
00198 mRosLightImageView.setImageDrawable(getResources().getDrawable(R.drawable.btn_radio_on_orange_light));
00199 } else if (status == RosStatus.MASTER_CONNECTED) {
00200
00201 mRosLightImageView.setImageDrawable(getResources().getDrawable(R.drawable.btn_radio_on_green_light));
00202
00203 if (mSnackbarRosReconnection != null && mSnackbarRosReconnection.isShown()) {
00204 mSnackbarRosReconnection.dismiss();
00205 }
00206
00207 mToolbarMenu.findItem(R.id.settings).setIcon(R.drawable.ic_settings_white_24dp);
00208 } else if (status == RosStatus.MASTER_NOT_CONNECTED) {
00209
00210 mRosLightImageView.setImageDrawable(getResources().getDrawable(R.drawable.btn_radio_on_red_light));
00211
00212 mSnackbarRosReconnection = Snackbar.make(findViewById(android.R.id.content),
00213 getString(R.string.snackbar_text_reconnect_ros), Snackbar.LENGTH_INDEFINITE);
00214 mSnackbarRosReconnection.setAction(getString(R.string.snackbar_action_text_reconnect_ros),
00215 new View.OnClickListener() {
00216 @Override
00217 public void onClick(View view) {
00218 mRunLocalMaster = mSharedPref.getBoolean(getString(R.string.pref_master_is_local_key), false);
00219 mMasterUri = mSharedPref.getString(getString(R.string.pref_master_uri_key),
00220 getResources().getString(R.string.pref_master_uri_default));
00221 mUriTextView.setText(mMasterUri);
00222 initAndStartRosJavaNode();
00223 }
00224 }
00225 );
00226 View snackBarView = mSnackbarRosReconnection.getView();
00227 snackBarView.setBackgroundColor(getResources().getColor(android.R.color.holo_red_dark));
00228 mSnackbarRosReconnection.show();
00229
00230 AnimatorSet set = (AnimatorSet) AnimatorInflater.loadAnimator(RunningActivity.this, R.animator.master_uri_text_animation);
00231 set.setTarget(mUriTextView);
00232 set.start();
00233
00234 mToolbarMenu.findItem(R.id.settings).setIcon(R.drawable.ic_settings_red_24dp);
00235 }
00236 }
00237 });
00238 }
00239
00240 private void updateTangoStatus(TangoStatus status) {
00241 if (mTangoStatus != status) {
00242 mTangoStatus = status;
00243 switchTangoLight(status);
00244 if (status == TangoStatus.NO_FIRST_VALID_POSE) {
00245 displayToastMessage(R.string.point_device);
00246 }
00247 }
00248 }
00249
00250 private void switchTangoLight(final TangoStatus status) {
00251 runOnUiThread(new Runnable() {
00252 @Override
00253 public void run() {
00254 if (status == TangoStatus.UNKNOWN) {
00255 mTangoLightImageView.setImageDrawable(getResources().getDrawable(R.drawable.btn_radio_on_orange_light));
00256 } else if (status == TangoStatus.SERVICE_CONNECTED) {
00257 mTangoLightImageView.setImageDrawable(getResources().getDrawable(R.drawable.btn_radio_on_green_light));
00258 } else {
00259 mTangoLightImageView.setImageDrawable(getResources().getDrawable(R.drawable.btn_radio_on_red_light));
00260 }
00261 }
00262 });
00263 }
00264
00265 private void updateLoadAndSaveMapButtons() {
00266 mCreateNewMap = mSharedPref.getBoolean(getString(R.string.pref_create_new_map_key), false);
00267 runOnUiThread(new Runnable() {
00268 @Override
00269 public void run() {
00270 mSaveMapButton.setEnabled(!mMapSaved);
00271 if (mCreateNewMap) {
00272 mSaveMapButton.setVisibility(View.VISIBLE);
00273 mLoadOccupancyGridButton.setVisibility(View.GONE);
00274 } else {
00275 mSaveMapButton.setVisibility(View.GONE);
00276 mLoadOccupancyGridButton.setVisibility(View.VISIBLE);
00277 }
00278 }
00279 });
00280 }
00281
00286 private void displayToastMessage(final int messageId) {
00287 runOnUiThread(new Runnable() {
00288 @Override
00289 public void run() {
00290 Toast.makeText(getApplicationContext(), messageId, Toast.LENGTH_LONG).show();
00291 }
00292 });
00293 }
00294
00295 private void showSaveMapDialog() {
00296 FragmentManager manager = getFragmentManager();
00297 SaveMapDialog saveMapDialog = new SaveMapDialog();
00298 saveMapDialog.setStyle(DialogFragment.STYLE_NORMAL, R.style.CustomDialog);
00299 saveMapDialog.show(manager, "SaveMapDialog");
00300 }
00301
00302 private void showLoadOccupancyGridDialog(boolean firstTry, java.util.ArrayList<java.lang.String> nameList) {
00303 FragmentManager manager = getFragmentManager();
00304 LoadOccupancyGridDialog loadOccupancyGridDialog = new LoadOccupancyGridDialog();
00305 Bundle bundle = new Bundle();
00306 bundle.putBoolean(getString(R.string.show_load_occupancy_grid_empty_key), nameList.isEmpty());
00307 bundle.putBoolean(getString(R.string.show_load_occupancy_grid_error_key), !firstTry);
00308 bundle.putStringArrayList(getString(R.string.list_names_occupancy_grid_key), nameList);
00309 loadOccupancyGridDialog.setArguments(bundle);
00310 loadOccupancyGridDialog.setStyle(DialogFragment.STYLE_NORMAL, R.style.CustomDialog);
00311 loadOccupancyGridDialog.show(manager, "LoadOccupancyGridDialog");
00312 }
00313
00314 private void setupUI() {
00315 setContentView(R.layout.running_activity);
00316 Toolbar toolbar = (Toolbar) findViewById(R.id.toolbar);
00317 setSupportActionBar(toolbar);
00318 mUriTextView = (TextView) findViewById(R.id.master_uri);
00319 mUriTextView.setText(mMasterUri);
00320 mRosLightImageView = (ImageView) findViewById(R.id.is_ros_ok_image);
00321 mTangoLightImageView = (ImageView) findViewById(R.id.is_tango_ok_image);
00322 mlogSwitch = (Switch) findViewById(R.id.log_switch);
00323 mlogSwitch.setOnCheckedChangeListener(new CompoundButton.OnCheckedChangeListener() {
00324 @Override
00325 public void onCheckedChanged(CompoundButton compoundButton, boolean isChecked) {
00326 mDisplayLog = isChecked;
00327 mLogTextView.setVisibility(isChecked ? View.VISIBLE : View.INVISIBLE);
00328 }
00329 });
00330 mLogTextView = (TextView)findViewById(R.id.log_view);
00331 mLogTextView.setMovementMethod(new ScrollingMovementMethod());
00332 mSaveMapButton = (Button) findViewById(R.id.save_map_button);
00333 mSaveMapButton.setOnClickListener(new View.OnClickListener() {
00334 @Override
00335 public void onClick(View view) {
00336 showSaveMapDialog();
00337 }
00338 });
00339 mLoadOccupancyGridButton = (Button) findViewById(R.id.load_occupancy_grid_button);
00340 mLoadOccupancyGridButton.setOnClickListener(new View.OnClickListener() {
00341 @Override
00342 public void onClick(View view) {
00343 mOccupancyGridNameList = new ArrayList<String>();
00344 try {
00345 String directory = mParameterNode.getStringParam(getString(R.string.occupancy_grid_directory_key));
00346 File occupancyGridDirectory = new File(directory);
00347 if (occupancyGridDirectory != null && occupancyGridDirectory.isDirectory()) {
00348 File[] files = occupancyGridDirectory.listFiles();
00349 for (File file : files) {
00350 if (FilenameUtils.getExtension(file.getName()).equals("yaml")) {
00351 mOccupancyGridNameList.add(FilenameUtils.removeExtension(file.getName()));
00352 }
00353 }
00354 }
00355 showLoadOccupancyGridDialog( true, mOccupancyGridNameList);
00356 } catch (RuntimeException e) {
00357 e.printStackTrace();
00358 }
00359 }
00360 });
00361 updateLoadAndSaveMapButtons();
00362 }
00363
00364 public void onClickOkSaveMapDialog(final String mapName) {
00365 assert(mapName !=null && !mapName.isEmpty());
00366 mSaveMapButton.setEnabled(false);
00367 new AsyncTask<Void, Void, Void>() {
00368 @Override
00369 protected Void doInBackground(Void... params) {
00370 mTangoServiceClientNode.callSaveMapService(mapName);
00371 return null;
00372 }
00373 }.execute();
00374 }
00375
00376 @Override
00377 public void onSaveMapServiceCallFinish(boolean success, final String message,
00378 final String mapName, final String mapUuid) {
00379 if (success) {
00380 mMapSaved = true;
00381 displayToastMessage(R.string.save_map_success);
00382 saveUuidsNamestoHashMap();
00383 runOnUiThread(new Runnable() {
00384 @Override
00385 public void run() {
00386 mSaveMapButton.setEnabled(!mMapSaved);
00387 mSnackbarLoadNewMap = Snackbar.make(findViewById(android.R.id.content),
00388 getString(R.string.snackbar_text_load_new_map), Snackbar.LENGTH_INDEFINITE);
00389 mSnackbarLoadNewMap.setAction(getString(R.string.snackbar_action_text_load_new_map), new View.OnClickListener() {
00390 @Override
00391 public void onClick(View view) {
00392 try {
00393 mParameterNode.changeSettingsToLocalizeInMap(mapUuid, getString(R.string.pref_create_new_map_key),
00394 getString(R.string.pref_localization_mode_key), getString(R.string.pref_localization_map_uuid_key));
00395 restartTango();
00396 } catch (RuntimeException e) {
00397 e.printStackTrace();
00398 }
00399 }
00400 });
00401 mSnackbarLoadNewMap.show();
00402 }
00403 });
00404 } else {
00405 Log.e(TAG, "Error while saving map: " + message);
00406 displayToastMessage(R.string.save_map_error);
00407 }
00408 }
00409
00410 public void onClickItemLoadOccupancyGridDialog(final String occupancyGridName) {
00411 assert(occupancyGridName !=null && !occupancyGridName.isEmpty());
00412 mLoadOccupancyGridButton.setEnabled(false);
00413 new AsyncTask<Void, Void, Void>() {
00414 @Override
00415 protected Void doInBackground(Void... params) {
00416 mTangoServiceClientNode.callLoadOccupancyGridService(occupancyGridName);
00417 return null;
00418 }
00419 }.execute();
00420 }
00421
00422 @Override
00423 public void onLoadOccupancyGridServiceCallFinish(boolean success, final String message,
00424 boolean aligned, String mapUuid) {
00425 if (success) {
00426 if (aligned) {
00427 displayToastMessage(R.string.load_occupancy_grid_success);
00428 } else {
00429 displayToastMessage(R.string.load_occupancy_grid_not_aligned);
00430 }
00431 Log.i(TAG, message);
00432 } else {
00433 Log.e(TAG, "Error while loading occupancy grid: " + message);
00434 displayToastMessage(R.string.load_occupancy_grid_error);
00435 showLoadOccupancyGridDialog( false, mOccupancyGridNameList);
00436 }
00437 runOnUiThread(new Runnable() {
00438 @Override
00439 public void run() {
00440 mLoadOccupancyGridButton.setEnabled(true);
00441 }
00442 });
00443 }
00444
00445 @Override
00446 public void onTangoConnectServiceFinish(int response, String message) {
00447 if (response != TangoConnectResponse.TANGO_SUCCESS) {
00448 Log.e(TAG, "Error connecting to Tango: " + response + ", message: " + message);
00449 displayToastMessage(R.string.tango_connect_error);
00450 return;
00451 }
00452 displayToastMessage(R.string.tango_connect_success);
00453 }
00454
00455 @Override
00456 public void onTangoDisconnectServiceFinish(int response, String message) {
00457 if (response != TangoConnectResponse.TANGO_SUCCESS) {
00458 Log.e(TAG, "Error disconnecting from Tango: " + response + ", message: " + message);
00459
00460
00461 displayToastMessage(R.string.tango_disconnect_error);
00462 return;
00463 }
00464 displayToastMessage(R.string.tango_disconnect_success);
00465 }
00466
00467 @Override
00468 public void onTangoReconnectServiceFinish(int response, String message) {
00469 if (response != TangoConnectResponse.TANGO_SUCCESS) {
00470 Log.e(TAG, "Error reconnecting to Tango: " + response + ", message: " + message);
00471 displayToastMessage(R.string.tango_reconnect_error);
00472 return;
00473 }
00474 displayToastMessage(R.string.tango_reconnect_success);
00475 }
00476
00477 public void onGetMapUuidsFinish(List<String> mapUuids, List<String> mapNames) {
00478 mUuidsNamesHashMap = new HashMap<>();
00479 if (mapUuids == null || mapNames == null) return;
00480 assert(mapUuids.size() == mapNames.size());
00481 for (int i = 0; i < mapUuids.size(); ++i) {
00482 mUuidsNamesHashMap.put(mapUuids.get(i), mapNames.get(i));
00483 }
00484 if(mParameterNode != null) {
00485 try {
00486 mParameterNode.setPreferencesFromParameterServer();
00487 } catch (RuntimeException e) {
00488 e.printStackTrace();
00489 }
00490 }
00491 Intent settingsActivityIntent = new Intent(SettingsActivity.NEW_UUIDS_NAMES_MAP_ALERT);
00492 settingsActivityIntent.putExtra(getString(R.string.uuids_names_map), mUuidsNamesHashMap);
00493 this.sendBroadcast(settingsActivityIntent);
00494 }
00495
00496 @Override
00497 public void onTangoStatus(int status) {
00498 if (status >= TangoStatus.values().length) {
00499 Log.e(TAG, "Invalid Tango status " + status);
00500 return;
00501 }
00502 if (status == TangoStatus.SERVICE_CONNECTED.ordinal() && mTangoStatus != TangoStatus.SERVICE_CONNECTED) {
00503 saveUuidsNamestoHashMap();
00504 try {
00505 mParameterNode.setPreferencesFromParameterServer();
00506 } catch (RuntimeException e) {
00507 e.printStackTrace();
00508 }
00509 mMapSaved = false;
00510 if (mSnackbarLoadNewMap != null && mSnackbarLoadNewMap.isShown()) {
00511 mSnackbarLoadNewMap.dismiss();
00512 }
00513 }
00514 updateLoadAndSaveMapButtons();
00515 updateTangoStatus(TangoStatus.values()[status]);
00516 }
00517
00518 private void saveUuidsNamestoHashMap() {
00519 mTangoServiceClientNode.callGetMapUuidsService();
00520 }
00521
00522 private void getTangoPermission(String permissionType, int requestCode) {
00523 Intent intent = new Intent();
00524 intent.setAction(REQUEST_TANGO_PERMISSION_ACTION);
00525 intent.putExtra(EXTRA_KEY_PERMISSIONTYPE, permissionType);
00526 startActivityForResult(intent, requestCode);
00527 }
00528
00529 @Override
00530 protected void onCreate(Bundle savedInstanceState) {
00531 super.onCreate(savedInstanceState);
00532 Thread.setDefaultUncaughtExceptionHandler(new Thread.UncaughtExceptionHandler() {
00533 public void uncaughtException(Thread t, Throwable e) {
00534 Log.e(TAG, "Uncaught exception of type " + e.getClass());
00535 e.printStackTrace();
00536 }
00537 });
00538
00539
00540 if (android.os.Build.VERSION.SDK_INT > 9) {
00541 StrictMode.ThreadPolicy policy = new StrictMode.ThreadPolicy.Builder().permitAll().build();
00542 StrictMode.setThreadPolicy(policy);
00543 }
00544 mSharedPref = PreferenceManager.getDefaultSharedPreferences(getBaseContext());
00545 mRunLocalMaster = mSharedPref.getBoolean(getString(R.string.pref_master_is_local_key), false);
00546 mMasterUri = mSharedPref.getString(getString(R.string.pref_master_uri_key),
00547 getResources().getString(R.string.pref_master_uri_default));
00548 mCreateNewMap = mSharedPref.getBoolean(getString(R.string.pref_create_new_map_key), false);
00549 String logFileName = mSharedPref.getString(getString(R.string.pref_log_file_key),
00550 getString(R.string.pref_log_file_default));
00551 setupUI();
00552 mLogger = new Logger(this, mLogTextView, TAGS_TO_LOG, logFileName, LOG_TEXT_MAX_LENGTH);
00553 }
00554
00555 @Override
00556 public void onConfigurationChanged(Configuration newConfig) {
00557 super.onConfigurationChanged(newConfig);
00558 setupUI();
00559 switchRosLight(mRosStatus);
00560 switchTangoLight(mTangoStatus);
00561 mlogSwitch.setChecked(mDisplayLog);
00562 mLogTextView.setText(mLogger.getLogText());
00563 }
00564
00565 @Override
00566 public boolean onCreateOptionsMenu(Menu menu) {
00567 MenuInflater inflater = getMenuInflater();
00568 inflater.inflate(R.menu.menu, menu);
00569 mToolbarMenu = menu;
00570 return true;
00571 }
00572
00573 @Override
00574 public boolean onOptionsItemSelected(MenuItem item) {
00575 switch (item.getItemId()) {
00576 case R.id.settings:
00577 if(mParameterNode != null) {
00578 try {
00579 mParameterNode.setPreferencesFromParameterServer();
00580 } catch (RuntimeException e) {
00581 e.printStackTrace();
00582 }
00583 }
00584 Intent settingsActivityIntent = new Intent(this, SettingsActivity.class);
00585 settingsActivityIntent.putExtra(getString(R.string.uuids_names_map), mUuidsNamesHashMap);
00586 startActivityForResult(settingsActivityIntent, StartSettingsActivityRequest.STANDARD_RUN);
00587 return true;
00588 case R.id.share:
00589 mLogger.saveLogToFile();
00590 Intent shareFileIntent = new Intent(Intent.ACTION_SEND);
00591 shareFileIntent.setType("text/plain");
00592 shareFileIntent.putExtra(Intent.EXTRA_STREAM, Uri.fromFile(mLogger.getLogFile()));
00593 startActivity(shareFileIntent);
00594 return true;
00595 default:
00596 return super.onOptionsItemSelected(item);
00597 }
00598 }
00599
00600 private void unbindFromTango() {
00601 if (TangoInitializationHelper.isTangoServiceBound()) {
00602 Log.i(TAG, "Unbind tango service");
00603 TangoInitializationHelper.unbindTangoService(this, mTangoServiceConnection);
00604 updateTangoStatus(TangoStatus.SERVICE_NOT_CONNECTED);
00605 }
00606 }
00607
00608 @Override
00609 protected void onDestroy() {
00610 super.onDestroy();
00611 if (mParameterNode != null) {
00612 try {
00613 mParameterNode.setPreferencesFromParameterServer();
00614 } catch (RuntimeException e) {
00615 e.printStackTrace();
00616 }
00617 }
00618 this.nodeMainExecutorService.forceShutdown();
00619 }
00620
00621 @Override
00622 protected void onActivityResult(int requestCode, int resultCode, Intent data) {
00623 if (resultCode == RESULT_CANCELED) {
00624
00625 if ((requestCode == StartSettingsActivityRequest.STANDARD_RUN ||
00626 requestCode == StartSettingsActivityRequest.FIRST_RUN) &&
00627 mParameterNode != null) {
00628 try {
00629 mParameterNode.uploadPreferencesToParameterServer();
00630 } catch (RuntimeException e) {
00631 e.printStackTrace();
00632 }
00633 }
00634
00635 if (data != null && data.getBooleanExtra(RESTART_TANGO, false)) {
00636 restartTango();
00637 }
00638
00639 if (requestCode == StartSettingsActivityRequest.FIRST_RUN) {
00640 mRunLocalMaster = mSharedPref.getBoolean(getString(R.string.pref_master_is_local_key), false);
00641 mMasterUri = mSharedPref.getString(getString(R.string.pref_master_uri_key),
00642 getResources().getString(R.string.pref_master_uri_default));
00643 mUriTextView.setText(mMasterUri);
00644 String logFileName = mSharedPref.getString(getString(R.string.pref_log_file_key),
00645 getString(R.string.pref_log_file_default));
00646 mLogger.setLogFileName(logFileName);
00647 mLogger.start();
00648 getTangoPermission(EXTRA_VALUE_ADF, REQUEST_CODE_ADF_PERMISSION);
00649 getTangoPermission(EXTRA_VALUE_DATASET, REQUEST_CODE_DATASET_PERMISSION);
00650 updateLoadAndSaveMapButtons();
00651 } else if (requestCode == StartSettingsActivityRequest.STANDARD_RUN) {
00652
00653 String logFileName = mSharedPref.getString(getString(R.string.pref_log_file_key),
00654 getString(R.string.pref_log_file_default));
00655 mLogger.setLogFileName(logFileName);
00656 if (mRosStatus == RosStatus.MASTER_NOT_CONNECTED && mSnackbarRosReconnection != null) {
00657
00658
00659 mSnackbarRosReconnection.show();
00660 }
00661 }
00662 }
00663
00664 if (requestCode == REQUEST_CODE_ADF_PERMISSION || requestCode == REQUEST_CODE_DATASET_PERMISSION) {
00665 if (resultCode == RESULT_CANCELED) {
00666
00667 displayToastMessage(R.string.tango_permission_denied);
00668 }
00669 if (requestCode == REQUEST_CODE_ADF_PERMISSION) {
00670
00671 mAdfPermissionHasBeenAnswered = true;
00672 }
00673 if (requestCode == REQUEST_CODE_DATASET_PERMISSION) {
00674
00675 mDatasetPermissionHasBeenAnswered = true;
00676 }
00677 if (mAdfPermissionHasBeenAnswered && mDatasetPermissionHasBeenAnswered) {
00678
00679
00680 Log.i(TAG, "initAndStartRosJavaNode");
00681 initAndStartRosJavaNode();
00682 }
00683
00684 }
00685 }
00686
00690 private void checkRosMasterConnection() {
00691 updateRosStatus(RosStatus.UNKNOWN);
00692 mRosConnectionLatch = new CountDownLatch(1);
00693 new MasterConnectionChecker(mMasterUri.toString(),
00694 new MasterConnectionChecker.UserHook() {
00695 @Override
00696 public void onSuccess(Object o) {
00697 updateRosStatus(RosStatus.MASTER_CONNECTED);
00698 mRosConnectionLatch.countDown();
00699 }
00700
00701 @Override
00702 public void onError(Throwable t) {
00703 updateRosStatus(RosStatus.MASTER_NOT_CONNECTED);
00704 Log.e(TAG, getString(R.string.ros_init_error));
00705 displayToastMessage(R.string.ros_init_error);
00706 mRosConnectionLatch.countDown();
00707 }
00708 },
00709 mRosConnectionLatch
00710 ).runTest();
00711 waitForLatchUnlock(mRosConnectionLatch, "ROS CONNECTION");
00712 }
00713
00714 private void restartTango() {
00715 if (mParameterNode != null) {
00716 try {
00717 mParameterNode.setPreferencesFromParameterServer();
00718 } catch (RuntimeException e) {
00719 e.printStackTrace();
00720 }
00721 }
00722 mTangoServiceClientNode.callTangoConnectService(TangoConnectRequest.RECONNECT);
00723 }
00724
00725 @Override
00726 protected void init(NodeMainExecutor nodeMainExecutor) {
00727 NodeConfiguration nodeConfiguration;
00728 try {
00729 nodeConfiguration = NodeConfiguration.newPublic(InetAddressFactory.newNonLoopback().getHostAddress());
00730 nodeConfiguration.setMasterUri(this.nodeMainExecutorService.getMasterUri());
00731 } catch (RosRuntimeException e) {
00732 e.printStackTrace();
00733 Log.e(TAG, getString(R.string.network_error));
00734 displayToastMessage(R.string.network_error);
00735 return;
00736 }
00737 checkRosMasterConnection();
00738 if (mRosStatus == RosStatus.MASTER_NOT_CONNECTED) {
00739 return;
00740 }
00741
00742 HashMap<String, String> tangoConfigurationParameters = new HashMap<String, String>();
00743 tangoConfigurationParameters.put(getString(R.string.pref_create_new_map_key), "boolean");
00744 tangoConfigurationParameters.put(getString(R.string.pref_enable_depth_key), "boolean");
00745 tangoConfigurationParameters.put(getString(R.string.pref_enable_color_camera_key), "boolean");
00746 tangoConfigurationParameters.put(getString(R.string.pref_localization_mode_key), "int_as_string");
00747 tangoConfigurationParameters.put(getString(R.string.pref_localization_map_uuid_key), "string");
00748 mParameterNode = new ParameterNode(this, tangoConfigurationParameters);
00749 nodeConfiguration.setNodeName(mParameterNode.getDefaultNodeName());
00750 nodeMainExecutor.execute(mParameterNode, nodeConfiguration);
00751
00752 mTangoServiceClientNode = new TangoServiceClientNode();
00753 mTangoServiceClientNode.setCallbackListener(this);
00754 nodeConfiguration.setNodeName(mTangoServiceClientNode.getDefaultNodeName());
00755 nodeMainExecutor.execute(mTangoServiceClientNode, nodeConfiguration);
00756
00757 mImuNode = new ImuNode(this);
00758 nodeConfiguration.setNodeName(mImuNode.getDefaultNodeName());
00759 nodeMainExecutor.execute(mImuNode, nodeConfiguration);
00760
00761 nodeConfiguration.setNodeName(TangoNodeletManager.NODE_NAME);
00762 if (TangoInitializationHelper.loadTangoSharedLibrary() !=
00763 TangoInitializationHelper.ARCH_ERROR &&
00764 TangoInitializationHelper.loadTangoRosNodeSharedLibrary()
00765 != TangoInitializationHelper.ARCH_ERROR) {
00766 mTangoNodeletManager = new TangoNodeletManager();
00767 TangoInitializationHelper.bindTangoService(this, mTangoServiceConnection);
00768 if (TangoInitializationHelper.isTangoVersionOk()) {
00769 nodeMainExecutor.execute(mTangoNodeletManager, nodeConfiguration, new ArrayList<NodeListener>(){{
00770 add(new DefaultNodeListener() {
00771 @Override
00772 public void onStart(ConnectedNode connectedNode) {
00773 int count = 0;
00774 while (count < MAX_TANGO_CONNECTION_TRY &&
00775 !mTangoServiceClientNode.callTangoConnectService(TangoConnectRequest.CONNECT)) {
00776 try {
00777 count++;
00778 Log.e(TAG, "Trying to connect to Tango, attempt " + count);
00779 Thread.sleep(200);
00780 } catch (InterruptedException e) {
00781 e.printStackTrace();
00782 }
00783 }
00784 if (count >= MAX_TANGO_CONNECTION_TRY) {
00785 updateTangoStatus(TangoStatus.SERVICE_NOT_CONNECTED);
00786 displayToastMessage(R.string.tango_connect_error);
00787 }
00788 }
00789 });
00790 }});
00791 } else {
00792 updateTangoStatus(TangoStatus.SERVICE_NOT_CONNECTED);
00793 Log.e(TAG, getResources().getString(R.string.tango_version_error));
00794 displayToastMessage(R.string.tango_version_error);
00795 }
00796 } else {
00797 updateTangoStatus(TangoStatus.SERVICE_NOT_CONNECTED);
00798 Log.e(TAG, getString(R.string.tango_lib_error));
00799 displayToastMessage(R.string.tango_lib_error);
00800 }
00801 }
00802
00808 @Override
00809 public void startMasterChooser() {
00810 boolean appPreviouslyStarted = mSharedPref.getBoolean(getString(R.string.pref_previously_started_key), false);
00811 if (appPreviouslyStarted) {
00812 mLogger.start();
00813 getTangoPermission(EXTRA_VALUE_ADF, REQUEST_CODE_ADF_PERMISSION);
00814 getTangoPermission(EXTRA_VALUE_DATASET, REQUEST_CODE_DATASET_PERMISSION);
00815 } else {
00816 Intent intent = new Intent(this, SettingsActivity.class);
00817 startActivityForResult(intent, StartSettingsActivityRequest.FIRST_RUN);
00818 }
00819 }
00820
00824 private void initAndStartRosJavaNode() {
00825 this.nodeMainExecutorService.addListener(new NodeMainExecutorServiceListener() {
00826 @Override
00827 public void onShutdown(NodeMainExecutorService nodeMainExecutorService) {
00828 unbindFromTango();
00829 mLogger.saveLogToFile();
00830
00831 android.os.Process.killProcess(android.os.Process.myPid());
00832 }
00833 });
00834 if (mRunLocalMaster) {
00835 try {
00836 this.nodeMainExecutorService.startMaster( false);
00837 mMasterUri = this.nodeMainExecutorService.getMasterUri().toString();
00838
00839
00840
00841
00842 WifiManager wifiManager = (WifiManager) getApplicationContext().getSystemService(WIFI_SERVICE);
00843 String deviceIP = Formatter.formatIpAddress(wifiManager.getConnectionInfo().getIpAddress());
00844 mUriTextView = (TextView) findViewById(R.id.master_uri);
00845 mUriTextView.setText("http://" + deviceIP + ":11311");
00846 } catch (RosRuntimeException e) {
00847 e.printStackTrace();
00848 Log.e(TAG, getString(R.string.local_master_error));
00849 displayToastMessage(R.string.local_master_error);
00850 return;
00851 }
00852 }
00853 if (mMasterUri != null) {
00854 URI masterUri;
00855 try {
00856 masterUri = URI.create(mMasterUri);
00857 } catch (IllegalArgumentException e) {
00858 Log.e(TAG, "Wrong URI: " + e.getMessage());
00859 return;
00860 }
00861 this.nodeMainExecutorService.setMasterUri(masterUri);
00862 new AsyncTask<Void, Void, Void>() {
00863 @Override
00864 protected Void doInBackground(Void... params) {
00865 RunningActivity.this.init(nodeMainExecutorService);
00866 return null;
00867 }
00868 }.execute();
00869 } else {
00870 Log.e(TAG, "Master URI is null");
00871 }
00872 }
00873
00879 private void waitForLatchUnlock(CountDownLatch latch, String latchName) {
00880 try {
00881 Log.i(TAG, "Waiting for " + latchName + " latch release...");
00882 latch.await();
00883 Log.i(TAG, latchName + " latch released!");
00884 } catch (InterruptedException ie) {
00885 Log.w(TAG, "Warning: continuing before " + latchName + " latch was released");
00886 }
00887 }
00888 }