Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 package com.github.rosjava.android_apps.map_nav;
00018
00019 import java.sql.Date;
00020 import java.text.DateFormat;
00021 import java.util.List;
00022 import java.util.concurrent.TimeUnit;
00023
00024 import android.app.AlertDialog;
00025 import android.app.ProgressDialog;
00026 import android.content.DialogInterface;
00027 import android.os.Bundle;
00028 import android.util.Log;
00029 import android.view.Menu;
00030 import android.view.MenuItem;
00031 import android.view.View;
00032 import android.view.ViewGroup;
00033 import android.widget.Button;
00034 import map_store.ListMapsResponse;
00035 import map_store.MapListEntry;
00036 import map_store.PublishMapResponse;
00037
00038 import com.github.rosjava.android_apps.application_management.RosAppActivity;
00039 import org.ros.android.view.RosImageView;
00040 import org.ros.android.view.visualization.layer.RobotLayer;
00041 import org.ros.namespace.NameResolver;
00042 import org.ros.node.NodeConfiguration;
00043 import org.ros.node.NodeMainExecutor;
00044 import org.ros.node.service.ServiceResponseListener;
00045 import org.ros.address.InetAddressFactory;
00046 import org.ros.android.BitmapFromCompressedImage;
00047 import org.ros.android.view.VirtualJoystickView;
00048 import org.ros.android.view.visualization.VisualizationView;
00049 import org.ros.android.view.visualization.layer.CameraControlListener;
00050 import org.ros.android.view.visualization.layer.OccupancyGridLayer;
00051 import org.ros.android.view.visualization.layer.LaserScanLayer;
00052 import org.ros.android.view.visualization.layer.PathLayer;
00053 import org.ros.android.view.visualization.layer.PoseSubscriberLayer;
00054 import org.ros.exception.RemoteException;
00055 import org.ros.time.NtpTimeProvider;
00056
00060 public class MainActivity extends RosAppActivity {
00061
00062 private static final String MAP_FRAME = "map";
00063 private static final String ROBOT_FRAME = "base_link";
00064 private static final String cameraTopic = "camera/rgb/image_color/compressed_throttle";
00065 private static final String virtualJoystickTopic = "android/virtual_joystick/cmd_vel";
00066 private static final String mapTopic = "map";
00067 private static final String scanTopic = "scan";
00068 private static final String pathLayerTopic = "move_base/TrajectoryPlannerROS/global_plan";
00069 private static final String initialPoseTopic = "initialpose";
00070
00071 private RosImageView<sensor_msgs.CompressedImage> cameraView;
00072 private VirtualJoystickView virtualJoystickView;
00073 private VisualizationView mapView;
00074 private ViewGroup mainLayout;
00075 private ViewGroup sideLayout;
00076 private Button backButton;
00077 private Button chooseMapButton;
00078 private MapPosePublisherLayer mapPosePublisherLayer;
00079 private ProgressDialog waitingDialog;
00080 private AlertDialog chooseMapDialog;
00081 private NodeMainExecutor nodeMainExecutor;
00082 private NodeConfiguration nodeConfiguration;
00083
00084 public MainActivity() {
00085
00086
00087
00088 super("Map nav", "Map nav");
00089 }
00090
00091 @SuppressWarnings("unchecked")
00092 @Override
00093 public void onCreate(Bundle savedInstanceState) {
00094
00095 String defaultRobotName = getString(R.string.default_robot);
00096 String defaultAppName = getString(R.string.default_app);
00097 setDefaultRobotName(defaultRobotName);
00098 setDefaultAppName(defaultAppName);
00099 setDashboardResource(R.id.top_bar);
00100 setMainWindowResource(R.layout.main);
00101 super.onCreate(savedInstanceState);
00102
00103 cameraView = (RosImageView<sensor_msgs.CompressedImage>) findViewById(R.id.image);
00104 cameraView.setMessageType(sensor_msgs.CompressedImage._TYPE);
00105 cameraView.setMessageToBitmapCallable(new BitmapFromCompressedImage());
00106 mapView = (VisualizationView) findViewById(R.id.map_view);
00107 virtualJoystickView = (VirtualJoystickView) findViewById(R.id.virtual_joystick);
00108 backButton = (Button) findViewById(R.id.back_button);
00109 chooseMapButton = (Button) findViewById(R.id.choose_map_button);
00110
00111 backButton.setOnClickListener(new View.OnClickListener() {
00112 @Override
00113 public void onClick(View view) {
00114 onBackPressed();
00115 }
00116 });
00117
00118 chooseMapButton.setOnClickListener(new View.OnClickListener() {
00119 @Override
00120 public void onClick(View view) {
00121 onChooseMapButtonPressed();
00122 }
00123 });
00124
00125 mapView.getCamera().jumpToFrame(ROBOT_FRAME);
00126 mainLayout = (ViewGroup) findViewById(R.id.main_layout);
00127 sideLayout = (ViewGroup) findViewById(R.id.side_layout);
00128
00129 }
00130
00131 @Override
00132 protected void init(NodeMainExecutor nodeMainExecutor) {
00133
00134 super.init(nodeMainExecutor);
00135
00136 this.nodeMainExecutor = nodeMainExecutor;
00137 nodeConfiguration = NodeConfiguration.newPublic(InetAddressFactory
00138 .newNonLoopback().getHostAddress(), getMasterUri());
00139
00140 NameResolver appNameSpace = getAppNameSpace();
00141 cameraView.setTopicName(appNameSpace.resolve(cameraTopic).toString());
00142 virtualJoystickView.setTopicName(appNameSpace.resolve(virtualJoystickTopic).toString());
00143
00144 nodeMainExecutor.execute(cameraView,
00145 nodeConfiguration.setNodeName("android/camera_view"));
00146 nodeMainExecutor.execute(virtualJoystickView,
00147 nodeConfiguration.setNodeName("android/virtual_joystick"));
00148
00149 ViewControlLayer viewControlLayer = new ViewControlLayer(this,
00150 nodeMainExecutor.getScheduledExecutorService(), cameraView,
00151 mapView, mainLayout, sideLayout);
00152
00153 viewControlLayer.addListener(new CameraControlListener() {
00154 @Override
00155 public void onZoom(double focusX, double focusY, double factor) {
00156
00157 }
00158
00159 @Override
00160 public void onTranslate(float distanceX, float distanceY) {
00161
00162 }
00163
00164 @Override
00165 public void onRotate(double focusX, double focusY, double deltaAngle) {
00166
00167 }
00168 });
00169
00170 mapView.addLayer(viewControlLayer);
00171 mapView.addLayer(new OccupancyGridLayer(appNameSpace.resolve(mapTopic).toString()));
00172 mapView.addLayer(new LaserScanLayer(appNameSpace.resolve(scanTopic).toString()));
00173 mapView.addLayer(new PathLayer(appNameSpace.resolve(pathLayerTopic).toString()));
00174 mapPosePublisherLayer = new MapPosePublisherLayer(appNameSpace, this);
00175 mapView.addLayer(mapPosePublisherLayer);
00176 mapView.addLayer(new InitialPoseSubscriberLayer(
00177 appNameSpace.resolve(initialPoseTopic).toString()));
00178 NtpTimeProvider ntpTimeProvider = new NtpTimeProvider(
00179 InetAddressFactory.newFromHostString("192.168.0.1"),
00180 nodeMainExecutor.getScheduledExecutorService());
00181 ntpTimeProvider.startPeriodicUpdates(1, TimeUnit.MINUTES);
00182 nodeConfiguration.setTimeProvider(ntpTimeProvider);
00183 nodeMainExecutor.execute(mapView, nodeConfiguration.setNodeName("android/map_view"));
00184
00185 readAvailableMapList();
00186 }
00187
00188 private void onChooseMapButtonPressed() {
00189 readAvailableMapList();
00190 }
00191
00192 public void setPoseClicked(View view) {
00193 setPose();
00194 }
00195
00196 public void setGoalClicked(View view) {
00197 setGoal();
00198 }
00199
00200 private void setPose() {
00201 mapPosePublisherLayer.setPoseMode();
00202 }
00203
00204 private void setGoal() {
00205 mapPosePublisherLayer.setGoalMode();
00206 }
00207
00208 private void readAvailableMapList() {
00209 safeShowWaitingDialog("Waiting...", "Waiting for map list");
00210
00211 MapManager mapManager = new MapManager();
00212 mapManager.setNameResolver(getAppNameSpace());
00213 mapManager.setFunction("list");
00214 safeShowWaitingDialog("Waiting...", "Waiting for map list");
00215 mapManager.setListService(new ServiceResponseListener<ListMapsResponse>() {
00216 @Override
00217 public void onSuccess(ListMapsResponse message) {
00218 Log.i("MapNav", "readAvailableMapList() Success");
00219 safeDismissWaitingDialog();
00220 showMapListDialog(message.getMapList());
00221 }
00222
00223 @Override
00224 public void onFailure(RemoteException e) {
00225 Log.i("MapNav", "readAvailableMapList() Failure");
00226 safeDismissWaitingDialog();
00227 }
00228 });
00229
00230 nodeMainExecutor.execute(mapManager,
00231 nodeConfiguration.setNodeName("android/list_maps"));
00232 }
00233
00237 private void showMapListDialog(final List<MapListEntry> list) {
00238
00239 final CharSequence[] availableMapNames = new CharSequence[list.size()];
00240 for (int i = 0; i < list.size(); i++) {
00241 String displayString;
00242 String name = list.get(i).getName();
00243 Date creationDate = new Date(list.get(i).getDate() * 1000);
00244 String dateTime = DateFormat.getDateTimeInstance(DateFormat.MEDIUM,
00245 DateFormat.SHORT).format(creationDate);
00246 if (name != null && !name.equals("")) {
00247 displayString = name + " " + dateTime;
00248 } else {
00249 displayString = dateTime;
00250 }
00251 availableMapNames[i] = displayString;
00252 }
00253
00254 runOnUiThread(new Runnable() {
00255 @Override
00256 public void run() {
00257 AlertDialog.Builder builder = new AlertDialog.Builder(
00258 MainActivity.this);
00259 builder.setTitle("Choose a map");
00260 builder.setItems(availableMapNames,
00261 new DialogInterface.OnClickListener() {
00262 @Override
00263 public void onClick(DialogInterface dialog,
00264 int itemIndex) {
00265 loadMap(list.get(itemIndex));
00266 }
00267 });
00268 chooseMapDialog = builder.create();
00269 chooseMapDialog.show();
00270 }
00271 });
00272 }
00273
00274 private void loadMap(MapListEntry mapListEntry) {
00275
00276 MapManager mapManager = new MapManager();
00277 mapManager.setNameResolver(getAppNameSpace());
00278 mapManager.setFunction("publish");
00279 mapManager.setMapId(mapListEntry.getMapId());
00280
00281 safeShowWaitingDialog("Waiting...", "Loading map");
00282 try {
00283 mapManager
00284 .setPublishService(new ServiceResponseListener<PublishMapResponse>() {
00285 @Override
00286 public void onSuccess(PublishMapResponse message) {
00287 Log.i("MapNav", "loadMap() Success");
00288 safeDismissWaitingDialog();
00289
00290 }
00291
00292 @Override
00293 public void onFailure(RemoteException e) {
00294 Log.i("MapNav", "loadMap() Failure");
00295 safeDismissWaitingDialog();
00296 }
00297 });
00298 } catch (Throwable ex) {
00299 Log.e("MapNav", "loadMap() caught exception.", ex);
00300 safeDismissWaitingDialog();
00301 }
00302 nodeMainExecutor.execute(mapManager,
00303 nodeConfiguration.setNodeName("android/publish_map"));
00304 }
00305
00306 private void safeDismissChooseMapDialog() {
00307 runOnUiThread(new Runnable() {
00308 @Override
00309 public void run() {
00310 if (chooseMapDialog != null) {
00311 chooseMapDialog.dismiss();
00312 chooseMapDialog = null;
00313 }
00314 }
00315 });
00316 }
00317
00318 private void showWaitingDialog(final CharSequence title,
00319 final CharSequence message) {
00320 dismissWaitingDialog();
00321 waitingDialog = ProgressDialog.show(MainActivity.this, title, message,
00322 true);
00323 waitingDialog.setProgressStyle(ProgressDialog.STYLE_SPINNER);
00324 }
00325
00326 private void dismissWaitingDialog() {
00327 if (waitingDialog != null) {
00328 waitingDialog.dismiss();
00329 waitingDialog = null;
00330 }
00331 }
00332
00333 private void safeShowWaitingDialog(final CharSequence title,
00334 final CharSequence message) {
00335 runOnUiThread(new Runnable() {
00336 @Override
00337 public void run() {
00338 showWaitingDialog(title, message);
00339 }
00340 });
00341 }
00342
00343 private void safeDismissWaitingDialog() {
00344 runOnUiThread(new Runnable() {
00345 @Override
00346 public void run() {
00347 dismissWaitingDialog();
00348 }
00349 });
00350 }
00351
00352 @Override
00353 public boolean onCreateOptionsMenu(Menu menu) {
00354 menu.add(0, 0, 0, R.string.stop_app);
00355 return super.onCreateOptionsMenu(menu);
00356 }
00357
00358 @Override
00359 public boolean onOptionsItemSelected(MenuItem item) {
00360 super.onOptionsItemSelected(item);
00361 switch (item.getItemId()) {
00362 case 0:
00363 onDestroy();
00364 break;
00365 }
00366 return true;
00367 }
00368 }