ImuPublisher.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 
00033 import java.util.List;
00034 
00035 import android.hardware.Sensor;
00036 import android.hardware.SensorEvent;
00037 import android.hardware.SensorEventListener;
00038 import android.hardware.SensorManager;
00039 import android.os.Looper;
00040 import android.os.SystemClock;
00041 
00042 import org.ros.node.ConnectedNode;
00043 import org.ros.message.Time;
00044 import org.ros.namespace.GraphName;
00045 import sensor_msgs.Imu;
00046 import org.ros.node.Node;
00047 import org.ros.node.NodeMain;
00048 import org.ros.node.topic.Publisher;
00049 
00054 public class ImuPublisher implements NodeMain
00055 {
00056 
00057   private ImuThread imuThread;
00058   private SensorListener sensorListener;
00059   private SensorManager sensorManager;
00060   private Publisher<Imu> publisher;
00061   
00062   private class ImuThread extends Thread
00063   {
00064           private final SensorManager sensorManager;
00065           private SensorListener sensorListener;
00066           private Looper threadLooper;
00067           
00068           private final Sensor accelSensor;
00069           private final Sensor gyroSensor;
00070           private final Sensor quatSensor;
00071           
00072           private ImuThread(SensorManager sensorManager, SensorListener sensorListener)
00073           {
00074                   this.sensorManager = sensorManager;
00075                   this.sensorListener = sensorListener;
00076                   this.accelSensor = this.sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
00077                   this.gyroSensor = this.sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE);
00078                   this.quatSensor = this.sensorManager.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR);
00079           }
00080           
00081             
00082           public void run()
00083           {
00084                         Looper.prepare();
00085                         this.threadLooper = Looper.myLooper();
00086                         this.sensorManager.registerListener(this.sensorListener, this.accelSensor, SensorManager.SENSOR_DELAY_FASTEST);
00087                         this.sensorManager.registerListener(this.sensorListener, this.gyroSensor, SensorManager.SENSOR_DELAY_FASTEST);
00088                         this.sensorManager.registerListener(this.sensorListener, this.quatSensor, SensorManager.SENSOR_DELAY_FASTEST);
00089                         Looper.loop();
00090           }
00091             
00092             
00093           public void shutdown()
00094           {
00095                 this.sensorManager.unregisterListener(this.sensorListener);
00096                 if(this.threadLooper != null)
00097                 {
00098                     this.threadLooper.quit();
00099                 }
00100           }
00101         }
00102   
00103   private class SensorListener implements SensorEventListener
00104   {
00105 
00106     private Publisher<Imu> publisher;
00107     
00108     private boolean hasAccel;
00109     private boolean hasGyro;
00110     private boolean hasQuat;
00111     
00112     private long accelTime;
00113     private long gyroTime;
00114     private long quatTime;
00115     
00116     private Imu imu;
00117 
00118     private SensorListener(Publisher<Imu> publisher, boolean hasAccel, boolean hasGyro, boolean hasQuat)
00119     {
00120       this.publisher = publisher;
00121       this.hasAccel = hasAccel;
00122       this.hasGyro = hasGyro;
00123       this.hasQuat = hasQuat;
00124       this.accelTime = 0;
00125       this.gyroTime = 0;
00126       this.quatTime = 0;
00127       this.imu = this.publisher.newMessage();
00128     }
00129 
00130 //      @Override
00131         public void onAccuracyChanged(Sensor sensor, int accuracy)
00132         {
00133         }
00134 
00135 //      @Override
00136         public void onSensorChanged(SensorEvent event)
00137         {
00138                 if(event.sensor.getType() == Sensor.TYPE_ACCELEROMETER)
00139                 {
00140                         this.imu.getLinearAcceleration().setX(event.values[0]);
00141                         this.imu.getLinearAcceleration().setY(event.values[1]);
00142                         this.imu.getLinearAcceleration().setZ(event.values[2]);
00143                         
00144                         double[] tmpCov = {0.01,0,0, 0,0.01,0, 0,0,0.01};// TODO Make Parameter
00145                         this.imu.setLinearAccelerationCovariance(tmpCov);
00146                         this.accelTime = event.timestamp;
00147                 }
00148                 else if(event.sensor.getType() == Sensor.TYPE_GYROSCOPE)
00149                 {
00150                         this.imu.getAngularVelocity().setX(event.values[0]);
00151                         this.imu.getAngularVelocity().setY(event.values[1]);
00152                         this.imu.getAngularVelocity().setZ(event.values[2]);
00153                         double[] tmpCov = {0.0025,0,0, 0,0.0025,0, 0,0,0.0025};// TODO Make Parameter
00154                         this.imu.setAngularVelocityCovariance(tmpCov);
00155                 this.gyroTime = event.timestamp;
00156                 }
00157                 else if(event.sensor.getType() == Sensor.TYPE_ROTATION_VECTOR)
00158                 {
00159                 float[] quaternion = new float[4];
00160                 SensorManager.getQuaternionFromVector(quaternion, event.values);
00161                 this.imu.getOrientation().setW(quaternion[0]);
00162                 this.imu.getOrientation().setX(quaternion[1]);
00163                 this.imu.getOrientation().setY(quaternion[2]);
00164                 this.imu.getOrientation().setZ(quaternion[3]);
00165                         double[] tmpCov = {0.001,0,0, 0,0.001,0, 0,0,0.001};// TODO Make Parameter
00166                         this.imu.setOrientationCovariance(tmpCov);
00167                 this.quatTime = event.timestamp;
00168                 }
00169                 
00170                 // Currently storing event times in case I filter them in the future.  Otherwise they are used to determine if all sensors have reported.
00171                 if((this.accelTime != 0 || !this.hasAccel) && (this.gyroTime != 0 || !this.hasGyro) && (this.quatTime != 0 || !this.hasQuat))
00172                 {
00173                         // Convert event.timestamp (nanoseconds uptime) into system time, use that as the header stamp
00174                         long time_delta_millis = System.currentTimeMillis() - SystemClock.uptimeMillis();
00175                         this.imu.getHeader().setStamp(Time.fromMillis(time_delta_millis + event.timestamp/1000000));
00176                         this.imu.getHeader().setFrameId("/imu");// TODO Make parameter
00177                         
00178                         publisher.publish(this.imu);
00179 
00180                         // Create a new message
00181                         this.imu = this.publisher.newMessage();
00182                         
00183                         // Reset times
00184                         this.accelTime = 0;
00185                         this.gyroTime = 0;
00186                         this.quatTime = 0;
00187                 }
00188         }
00189   }
00190 
00191   
00192   public ImuPublisher(SensorManager manager)
00193   {
00194           this.sensorManager = manager;
00195   }
00196 
00197   public GraphName getDefaultNodeName()
00198   {
00199             return GraphName.of("android_sensors_driver/imuPublisher");
00200   }
00201   
00202   public void onError(Node node, Throwable throwable)
00203   {
00204   }
00205 
00206   public void onStart(ConnectedNode node)
00207   {
00208           try
00209           {
00210                         this.publisher = node.newPublisher("android/imu", "sensor_msgs/Imu");
00211                         //      Determine if we have the various needed sensors
00212                         boolean hasAccel = false;
00213                         boolean hasGyro = false;
00214                         boolean hasQuat = false;
00215         
00216                         List<Sensor> accelList = this.sensorManager.getSensorList(Sensor.TYPE_ACCELEROMETER);
00217                         
00218                         if(accelList.size() > 0)
00219                         {
00220                                 hasAccel = true;
00221                         }
00222                         
00223                         List<Sensor> gyroList = this.sensorManager.getSensorList(Sensor.TYPE_GYROSCOPE);
00224                         if(gyroList.size() > 0)
00225                         {
00226                                 hasGyro = true;
00227                         }
00228                         
00229                         List<Sensor> quatList = this.sensorManager.getSensorList(Sensor.TYPE_ROTATION_VECTOR);
00230                         if(quatList.size() > 0)
00231                         {
00232                                 hasQuat = true;
00233                         }
00234                         
00235                         this.sensorListener = new SensorListener(publisher, hasAccel, hasGyro, hasQuat);
00236                         this.imuThread = new ImuThread(this.sensorManager, sensorListener);
00237                         this.imuThread.start();         
00238           }
00239           catch (Exception e)
00240           {
00241                   if (node != null)
00242                   {
00243                           node.getLog().fatal(e);
00244                   }
00245                   else
00246                   {
00247                           e.printStackTrace();
00248                   }
00249           }
00250   }
00251 
00252 //@Override
00253   public void onShutdown(Node arg0)
00254   {
00255           this.imuThread.shutdown();
00256         
00257           try
00258           {
00259                   this.imuThread.join();
00260           }
00261           catch (InterruptedException e)
00262           {
00263                   e.printStackTrace();
00264           }
00265   }
00266 
00267 //@Override
00268   public void onShutdownComplete(Node arg0)
00269   {
00270   }
00271 
00272 }
00273 


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