Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 package com.ros.turtlebot.apps.core_components;
00036
00037 import java.util.HashMap;
00038 import java.util.List;
00039
00040 import org.ros.android.robotapp.Dashboard.DashboardInterface;
00041 import org.ros.android.view.BatteryLevelView;
00042 import org.ros.exception.RemoteException;
00043 import org.ros.exception.RosException;
00044 import org.ros.message.MessageListener;
00045 import org.ros.namespace.GraphName;
00046 import org.ros.namespace.NameResolver;
00047 import org.ros.node.ConnectedNode;
00048 import org.ros.node.Node;
00049 import org.ros.node.service.ServiceClient;
00050 import org.ros.node.service.ServiceResponseListener;
00051 import org.ros.node.topic.Subscriber;
00052
00053 import create_node.SetDigitalOutputsRequest;
00054 import create_node.SetDigitalOutputsResponse;
00055 import create_node.SetTurtlebotModeRequest;
00056 import create_node.SetTurtlebotModeResponse;
00057 import create_node.TurtlebotSensorState;
00058
00059 import diagnostic_msgs.DiagnosticArray;
00060 import diagnostic_msgs.DiagnosticStatus;
00061 import diagnostic_msgs.KeyValue;
00062
00063 import android.content.Context;
00064 import android.graphics.Color;
00065 import android.util.AttributeSet;
00066 import android.util.Log;
00067 import android.view.LayoutInflater;
00068 import android.view.View;
00069 import android.widget.ImageButton;
00070 import android.widget.LinearLayout;
00071 import android.widget.ProgressBar;
00072 import android.widget.Toast;
00073
00074 public class TurtlebotDashboard extends LinearLayout implements DashboardInterface {
00075 private ImageButton modeButton;
00076 private ProgressBar modeWaitingSpinner;
00077 private BatteryLevelView robotBattery;
00078 private BatteryLevelView laptopBattery;
00079 private ConnectedNode connectedNode;
00080 private Subscriber<diagnostic_msgs.DiagnosticArray> diagnosticSubscriber;
00081 private boolean powerOn = false;
00082 private int numModeResponses;
00083 private int numModeErrors;
00084
00085 public TurtlebotDashboard(Context context) {
00086 super(context);
00087 inflateSelf(context);
00088 }
00089 public TurtlebotDashboard(Context context, AttributeSet attrs) {
00090 super(context, attrs);
00091 inflateSelf(context);
00092 }
00093 private void inflateSelf(Context context) {
00094 LayoutInflater inflater = (LayoutInflater) context.getSystemService(Context.LAYOUT_INFLATER_SERVICE);
00095 inflater.inflate(R.layout.turtlebot_dashboard, this);
00096 modeButton = (ImageButton) findViewById(R.id.mode_button);
00097 modeButton.setOnClickListener(new OnClickListener() {
00098 @Override
00099 public void onClick(View v) {
00100 onModeButtonClicked();
00101 }
00102 });
00103 modeWaitingSpinner = (ProgressBar) findViewById(R.id.mode_waiting_spinner);
00104 modeWaitingSpinner.setIndeterminate(true);
00105 modeWaitingSpinner.setVisibility(View.GONE);
00106 robotBattery = (BatteryLevelView) findViewById(R.id.robot_battery);
00107 laptopBattery = (BatteryLevelView) findViewById(R.id.laptop_battery);
00108 }
00119 private void handleDiagnosticArray(DiagnosticArray msg) {
00120 String mode = null;
00121 for(DiagnosticStatus status : msg.getStatus()) {
00122 if(status.getName().equals("/Power System/Battery")) {
00123 populateBatteryFromStatus(robotBattery, status);
00124 }
00125 if(status.getName().equals("/Power System/Laptop Battery")) {
00126 populateBatteryFromStatus(laptopBattery, status);
00127 }
00128 if(status.getName().equals("/Mode/Operating Mode")) {
00129 mode = status.getMessage();
00130 }
00131 }
00132 showMode(mode);
00133 }
00134
00135
00136 private void onModeButtonClicked() {
00137 powerOn = !powerOn;
00138 SetTurtlebotModeRequest modeRequest = connectedNode.getTopicMessageFactory().newFromType(SetTurtlebotModeRequest._TYPE);
00139 SetDigitalOutputsRequest setDigOutRequest = connectedNode.getTopicMessageFactory().newFromType(SetDigitalOutputsRequest._TYPE);
00140 setDigOutRequest.setDigitalOut1((byte) 0);
00141 setDigOutRequest.setDigitalOut2((byte) 0);
00142 if(powerOn) {
00143 modeRequest.setMode(TurtlebotSensorState.OI_MODE_FULL);
00144 setDigOutRequest.setDigitalOut0((byte) 1);
00145 } else {
00146 modeRequest.setMode(TurtlebotSensorState.OI_MODE_PASSIVE);
00147 setDigOutRequest.setDigitalOut0((byte) 0);
00148 }
00149 setModeWaiting(true);
00150 numModeResponses = 0;
00151 numModeErrors = 0;
00152
00153 try {
00154 ServiceClient<SetTurtlebotModeRequest, SetTurtlebotModeResponse> modeServiceClient = connectedNode.newServiceClient("turtlebot_node/set_operation_mode", "turtlebot_node/SetTurtlebotMode");
00155 modeServiceClient.call(modeRequest, new ServiceResponseListener<SetTurtlebotModeResponse>() {
00156 @Override
00157 public void onSuccess(SetTurtlebotModeResponse message) {
00158 numModeResponses++;
00159 updateModeWaiting();
00160 }
00161 @Override
00162 public void onFailure(RemoteException e) {
00163 numModeResponses++;
00164 numModeErrors++;
00165 updateModeWaiting();
00166 }
00167 });
00168 } catch(Exception ex) {
00169 Toast.makeText(getContext(), "Exception in service call for set_operation_mode: " + ex.getMessage(), Toast.LENGTH_LONG).show();
00170 Log.i("TurtlebotDashboard", "making toast.");
00171 }
00172 try {
00173 ServiceClient<SetDigitalOutputsRequest, SetDigitalOutputsResponse> setDigOutServiceClient = connectedNode.newServiceClient("turtlebot_node/set_digital_outputs", "turtlebot_node/SetDigitalOutputs");
00174 setDigOutServiceClient.call(setDigOutRequest, new ServiceResponseListener<SetDigitalOutputsResponse>() {
00175 @Override
00176 public void onSuccess(final SetDigitalOutputsResponse msg) {
00177 numModeResponses++;
00178 updateModeWaiting();
00179 }
00180 @Override
00181 public void onFailure(RemoteException e) {
00182 numModeResponses++;
00183 numModeErrors++;
00184 updateModeWaiting();
00185 }
00186 });
00187 } catch(Exception ex) {
00188 Toast.makeText(getContext(), "Exception in service call for set_digital_outputs: " + ex.getMessage(), Toast.LENGTH_LONG).show();
00189 Log.i("TurtlebotDashboard", "making toast.");
00190 }
00191 }
00192
00193 private void updateModeWaiting() {
00194 if(numModeResponses >= 2) {
00195 setModeWaiting(false);
00196 }
00197 }
00198 private void setModeWaiting(final boolean waiting) {
00199 post(new Runnable() {
00200 @Override
00201 public void run() {
00202 modeWaitingSpinner.setVisibility(waiting ? View.VISIBLE : View.GONE);
00203 }
00204 });
00205 }
00206 private void showMode(String mode) {
00207 if(mode == null) {
00208 modeButton.setColorFilter(Color.GRAY);
00209 } else if(mode.equals("Full")) {
00210 modeButton.setColorFilter(Color.GREEN);
00211 powerOn = true;
00212 } else if(mode.equals("Safe")) {
00213 modeButton.setColorFilter(Color.YELLOW);
00214 powerOn = true;
00215 } else if(mode.equals("Passive")) {
00216 modeButton.setColorFilter(Color.RED);
00217 powerOn = false;
00218 } else {
00219 modeButton.setColorFilter(Color.GRAY);
00220 Log.w("TurtlebotDashboard", "Unknown mode string: '" + mode + "'");
00221 }
00222 setModeWaiting(false);
00223 }
00224 private void populateBatteryFromStatus(BatteryLevelView view, DiagnosticStatus status) {
00225 HashMap<String, String> values = keyValueArrayToMap(status.getValues());
00226 try {
00227 float percent = 100 * Float.parseFloat(values.get("Charge (Ah)")) / Float.parseFloat(values.get("Capacity (Ah)"));
00228 view.setBatteryPercent((int) percent);
00229
00230
00231
00232 } catch(NumberFormatException ex) {
00233
00234 } catch(ArithmeticException ex) {
00235
00236 } catch(NullPointerException ex) {
00237
00238 }
00239 try {
00240 view.setPluggedIn(Float.parseFloat(values.get("Current (A)")) > 0);
00241 } catch(NumberFormatException ex) {
00242 } catch(ArithmeticException ex) {
00243 } catch(NullPointerException ex) {
00244 }
00245 }
00246 private HashMap<String, String> keyValueArrayToMap(List<KeyValue> list) {
00247 HashMap<String, String> map = new HashMap<String, String>();
00248 for(KeyValue kv : list) {
00249 map.put(kv.getKey(), kv.getValue());
00250 }
00251 return map;
00252 }
00253
00254 @Override
00255 public void onShutdown(Node node) {
00256 if(diagnosticSubscriber != null) {
00257 diagnosticSubscriber.shutdown();
00258 }
00259 diagnosticSubscriber = null;
00260 connectedNode = null;
00261
00262 }
00263
00264 @Override
00265 public void onStart(ConnectedNode connectedNode) {
00266
00267 this.connectedNode = connectedNode;
00268 try {
00269 diagnosticSubscriber = connectedNode.newSubscriber("diagnostics_agg", "diagnostic_msgs/DiagnosticArray");
00270 diagnosticSubscriber.addMessageListener(new MessageListener<diagnostic_msgs.DiagnosticArray>() {
00271 @Override
00272 public void onNewMessage(final diagnostic_msgs.DiagnosticArray message) {
00273 TurtlebotDashboard.this.post(new Runnable() {
00274 @Override
00275 public void run() {
00276 TurtlebotDashboard.this.handleDiagnosticArray(message);
00277 }
00278 });
00279 }
00280 });
00281 NameResolver resolver = connectedNode.getResolver().newChild(GraphName.of("/turtlebot_node"));
00282 } catch(Exception ex) {
00283 this.connectedNode = null;
00284 try {
00285 throw (new RosException(ex));
00286 } catch (RosException e) {
00287
00288 e.printStackTrace();
00289 }
00290 }
00291 }
00292 }