NavSatFixPublisher.java
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Chad Rockey
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Android Sensors Driver nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 package org.ros.android.android_sensors_driver;
00031 
00032 import android.location.Location;
00033 import android.location.LocationListener;
00034 import android.location.LocationManager;
00035 import android.location.LocationProvider;
00036 import android.os.Bundle;
00037 import android.os.Looper;
00038 
00039 import org.ros.namespace.GraphName;
00040 import org.ros.node.ConnectedNode;
00041 import org.ros.message.Time;
00042 import sensor_msgs.NavSatFix;
00043 import sensor_msgs.NavSatStatus;
00044 import org.ros.node.Node;
00045 import org.ros.node.NodeMain;
00046 import org.ros.node.topic.Publisher;
00047 
00052 public class NavSatFixPublisher implements NodeMain {
00053 
00054   private NavSatThread navSatThread;
00055   private LocationManager locationManager;
00056   private NavSatListener navSatFixListener;
00057   private Publisher<NavSatFix> publisher;
00058   
00059   private class NavSatThread extends Thread {
00060           LocationManager locationManager;
00061           NavSatListener navSatListener;
00062           private Looper threadLooper;
00063           
00064           private NavSatThread(LocationManager locationManager, NavSatListener navSatListener){
00065                   this.locationManager = locationManager;
00066                   this.navSatListener = navSatListener;
00067           }
00068           
00069             public void run() {
00070                 Looper.prepare();
00071                 threadLooper = Looper.myLooper();
00072                 this.locationManager.requestLocationUpdates(LocationManager.GPS_PROVIDER, 0, 0, this.navSatListener);
00073                 Looper.loop();
00074             }
00075             
00076             public void shutdown(){
00077                 this.locationManager.removeUpdates(this.navSatListener);
00078                 if(threadLooper != null){
00079                     threadLooper.quit();
00080                 }
00081             }
00082         }
00083   
00084   private class NavSatListener implements LocationListener {
00085 
00086     private Publisher<NavSatFix> publisher;
00087 
00088     private volatile byte currentStatus;
00089 
00090     private NavSatListener(Publisher<NavSatFix> publisher) {
00091       this.publisher = publisher;
00092       this.currentStatus = NavSatStatus.STATUS_FIX; // Default to fix until we are told otherwise.
00093     }
00094 
00095 //      @Override
00096         public void onLocationChanged(Location location)
00097         {
00098                 NavSatFix fix = this.publisher.newMessage();
00099                 fix.getHeader().setStamp(Time.fromMillis(System.currentTimeMillis()));
00100                 fix.getHeader().setFrameId("/gps");
00101                 
00102                 fix.getStatus().setStatus(currentStatus);
00103                 fix.getStatus().setService(NavSatStatus.SERVICE_GPS);
00104                 
00105                 fix.setLatitude(location.getLatitude());
00106                 fix.setLongitude(location.getLongitude());
00107                 fix.setAltitude(location.getAltitude());
00108                 fix.setPositionCovarianceType(NavSatFix.COVARIANCE_TYPE_APPROXIMATED);
00109                 double deviation = location.getAccuracy();
00110                 double covariance = deviation*deviation;
00111                 double[] tmpCov = {covariance,0,0, 0,covariance,0, 0,0,covariance};
00112                 fix.setPositionCovariance(tmpCov);
00113                 publisher.publish(fix);
00114         }
00115 
00116 //      @Override
00117         public void onProviderDisabled(String provider) {
00118         }
00119 
00120 //      @Override
00121         public void onProviderEnabled(String provider) {
00122         }
00123 
00124 //      @Override
00125         public void onStatusChanged(String provider, int status, Bundle extras) {
00126                 switch (status) {
00127                 case LocationProvider.OUT_OF_SERVICE:
00128                         currentStatus = NavSatStatus.STATUS_NO_FIX;
00129                         break;
00130                 case LocationProvider.TEMPORARILY_UNAVAILABLE:
00131                         currentStatus = NavSatStatus.STATUS_NO_FIX;
00132                         break;
00133                 case LocationProvider.AVAILABLE:
00134                         currentStatus = NavSatStatus.STATUS_FIX;
00135                         break;
00136                 }
00137         }
00138   }
00139 
00140   public NavSatFixPublisher(LocationManager manager) {
00141           this.locationManager = manager;
00142   }
00143 
00144 //@Override
00145 public void onStart(ConnectedNode node)
00146 {
00147   try
00148   {
00149         this.publisher = node.newPublisher("android/fix", "sensor_msgs/NavSatFix");
00150         this.navSatFixListener = new NavSatListener(publisher);
00151         this.navSatThread = new NavSatThread(this.locationManager, this.navSatFixListener);
00152         this.navSatThread.start();
00153   }
00154   catch (Exception e)
00155   {
00156     if (node != null)
00157     {
00158       node.getLog().fatal(e);
00159     }
00160     else
00161     {
00162       e.printStackTrace();
00163     }
00164   }
00165 }
00166 
00167 //@Override
00168 public void onShutdown(Node arg0) {
00169         this.navSatThread.shutdown();
00170         try {
00171                 this.navSatThread.join();
00172         } catch (InterruptedException e) {
00173                 e.printStackTrace();
00174         }
00175 }
00176 
00177 //@Override
00178 public void onShutdownComplete(Node arg0) {
00179 }
00180 
00181 public GraphName getDefaultNodeName()
00182 {
00183             return GraphName.of("android_sensors_driver/imuPublisher");
00184 }
00185 
00186 public void onError(Node node, Throwable throwable)
00187 {
00188 }
00189 
00190 }


android_sensors_driver
Author(s): Chad Rockey (chadrockey@gmail.com)
autogenerated on Sun Oct 5 2014 22:10:14