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 org.ros.android.android_tutorial_map_viewer;
00018
00019 import com.google.common.base.Preconditions;
00020
00021 import android.os.Bundle;
00022 import android.view.View;
00023 import android.view.Window;
00024 import android.view.WindowManager;
00025 import android.widget.Toast;
00026 import android.widget.ToggleButton;
00027 import org.ros.address.InetAddressFactory;
00028 import org.ros.android.RosActivity;
00029 import org.ros.android.view.visualization.VisualizationView;
00030 import org.ros.android.view.visualization.layer.CameraControlLayer;
00031 import org.ros.android.view.visualization.layer.CameraControlListener;
00032 import org.ros.android.view.visualization.layer.CompressedOccupancyGridLayer;
00033 import org.ros.android.view.visualization.layer.OccupancyGridLayer;
00034 import org.ros.android.view.visualization.layer.LaserScanLayer;
00035 import org.ros.android.view.visualization.layer.RobotLayer;
00036 import org.ros.node.NodeConfiguration;
00037 import org.ros.node.NodeMainExecutor;
00038 import org.ros.time.NtpTimeProvider;
00039
00040 import java.util.concurrent.TimeUnit;
00041
00042 public class MainActivity extends RosActivity {
00043
00044 private static final String MAP_FRAME = "map";
00045 private static final String ROBOT_FRAME = "base_link";
00046
00047 private final SystemCommands systemCommands;
00048
00049 private VisualizationView visualizationView;
00050 private ToggleButton followMeToggleButton;
00051
00052 public MainActivity() {
00053 super("Map Viewer", "Map Viewer");
00054 systemCommands = new SystemCommands();
00055 }
00056
00057 @Override
00058 public void onCreate(Bundle savedInstanceState) {
00059 super.onCreate(savedInstanceState);
00060 requestWindowFeature(Window.FEATURE_NO_TITLE);
00061 getWindow().setFlags(WindowManager.LayoutParams.FLAG_FULLSCREEN,
00062 WindowManager.LayoutParams.FLAG_FULLSCREEN);
00063 setContentView(R.layout.main);
00064 visualizationView = (VisualizationView) findViewById(R.id.visualization);
00065 followMeToggleButton = (ToggleButton) findViewById(R.id.follow_me_toggle_button);
00066 enableFollowMe();
00067 }
00068
00069 @Override
00070 protected void init(NodeMainExecutor nodeMainExecutor) {
00071 CameraControlLayer cameraControlLayer =
00072 new CameraControlLayer(this, nodeMainExecutor.getScheduledExecutorService());
00073 cameraControlLayer.addListener(new CameraControlListener() {
00074 @Override
00075 public void onZoom(double focusX, double focusY, double factor) {
00076 disableFollowMe();
00077 }
00078
00079 @Override
00080 public void onTranslate(float distanceX, float distanceY) {
00081 disableFollowMe();
00082 }
00083
00084 @Override
00085 public void onRotate(double focusX, double focusY, double deltaAngle) {
00086 disableFollowMe();
00087 }
00088
00089 });
00090 visualizationView.addLayer(cameraControlLayer);
00091 visualizationView.addLayer(new CompressedOccupancyGridLayer("map/png"));
00092 visualizationView.addLayer(new LaserScanLayer("scan"));
00093
00094
00095
00096 visualizationView.addLayer(new RobotLayer(ROBOT_FRAME));
00097 NodeConfiguration nodeConfiguration =
00098 NodeConfiguration.newPublic(InetAddressFactory.newNonLoopback().getHostAddress(),
00099 getMasterUri());
00100 NtpTimeProvider ntpTimeProvider =
00101 new NtpTimeProvider(InetAddressFactory.newFromHostString("192.168.0.1"),
00102 nodeMainExecutor.getScheduledExecutorService());
00103 ntpTimeProvider.startPeriodicUpdates(1, TimeUnit.MINUTES);
00104 nodeConfiguration.setTimeProvider(ntpTimeProvider);
00105 nodeMainExecutor.execute(visualizationView, nodeConfiguration);
00106 nodeMainExecutor.execute(systemCommands, nodeConfiguration);
00107 }
00108
00109 public void onClearMapButtonClicked(View view) {
00110 toast("Clearing map...");
00111 systemCommands.reset();
00112 enableFollowMe();
00113 }
00114
00115 public void onSaveMapButtonClicked(View view) {
00116 toast("Saving map...");
00117 systemCommands.saveGeotiff();
00118 }
00119
00120 private void toast(final String text) {
00121 runOnUiThread(new Runnable() {
00122 @Override
00123 public void run() {
00124 Toast toast = Toast.makeText(MainActivity.this, text, Toast.LENGTH_SHORT);
00125 toast.show();
00126 }
00127 });
00128 }
00129
00130 public void onFollowMeToggleButtonClicked(View view) {
00131 boolean on = ((ToggleButton) view).isChecked();
00132 if (on) {
00133 enableFollowMe();
00134 } else {
00135 disableFollowMe();
00136 }
00137 }
00138
00139 private void enableFollowMe() {
00140 Preconditions.checkNotNull(visualizationView);
00141 Preconditions.checkNotNull(followMeToggleButton);
00142 runOnUiThread(new Runnable() {
00143 @Override
00144 public void run() {
00145 visualizationView.getCamera().jumpToFrame(ROBOT_FRAME);
00146 followMeToggleButton.setChecked(true);
00147 }
00148 });
00149 }
00150
00151 private void disableFollowMe() {
00152 Preconditions.checkNotNull(visualizationView);
00153 Preconditions.checkNotNull(followMeToggleButton);
00154 runOnUiThread(new Runnable() {
00155 @Override
00156 public void run() {
00157 visualizationView.getCamera().setFrame(MAP_FRAME);
00158 followMeToggleButton.setChecked(false);
00159 }
00160 });
00161 }
00162 }