TurtlebotDashboard.java
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  * Copyright (c) 2011, Willow Garage, Inc.
00005  * All rights reserved.
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions
00008  * are met:
00009  *
00010  *  * Redistributions of source code must retain the above copyright
00011  *    notice, this list of conditions and the following disclaimer.
00012  *  * Redistributions in binary form must reproduce the above
00013  *    copyright notice, this list of conditions and the following
00014  *    disclaimer in the documentation and/or other materials provided
00015  *    with the distribution.
00016  *  * Neither the name of Willow Garage, Inc. nor the names of its
00017  *    contributors may be used to endorse or promote products derived
00018  *    from this software without specific prior written permission.
00019  *
00020  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031  * POSSIBILITY OF SUCH DAMAGE.
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); // main breaker on
00145             } else {
00146                     modeRequest.setMode(TurtlebotSensorState.OI_MODE_PASSIVE);
00147                     setDigOutRequest.setDigitalOut0((byte) 0); // main breaker off
00148             }
00149             setModeWaiting(true);
00150             numModeResponses = 0;
00151             numModeErrors = 0;
00152             // TODO: can't I save the modeServiceClient? Causes trouble.
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                     // TODO: set color red/yellow/green based on level (maybe with
00230                     // level-set
00231                     // in XML)
00232             } catch(NumberFormatException ex) {
00233                     // TODO: make battery level gray
00234             } catch(ArithmeticException ex) {
00235                     // TODO: make battery level gray
00236             } catch(NullPointerException ex) {
00237                     // Do nothing: data wasn't there.
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                                         // TODO Auto-generated catch block
00288                                         e.printStackTrace();
00289                                 }
00290         }
00291         }
00292 }


turtlebot_android_core_components
Author(s): KazutoMurase
autogenerated on Mon Oct 6 2014 07:58:46