00001 package org.ros.android.androidSensorMessage;
00002
00003
00004 import java.util.List;
00005
00006 import org.ros.message.Time;
00007 import org.ros.namespace.GraphName;
00008 import org.ros.node.ConnectedNode;
00009 import org.ros.node.NodeMain;
00010 import org.ros.node.topic.Publisher;
00011
00012 import android.hardware.SensorEventListener;
00013 import android.hardware.SensorManager;
00014 import android.hardware.Sensor;
00015 import android.hardware.SensorEvent;
00016
00017 import android.os.Looper;
00018 import android.os.SystemClock;
00019 import android.util.Log;
00020 import android.widget.Button;
00021 import android.widget.TextView;
00022 import jsk_gui_msgs.DeviceSensor;
00023 import jsk_gui_msgs.Gravity;
00024 import jsk_gui_msgs.MagneticField;
00025 import sensor_msgs.Imu;
00026 import android.view.View;
00027
00028 import org.ros.node.*;
00029
00030 public class AndroidSensorMessageNode implements NodeMain {
00031
00032
00033 private SensorManager sensorManager;
00034 private SensorListener sensorListener;
00035 private AndroidSensorMessageView androidSensorMessageView;
00036 private List<Sensor> sensors;
00037 private int[] publishedSensor;
00038 private int currentView = 0;
00039 private TextView textViewX;
00040 private TextView textViewY;
00041 private TextView textViewZ;
00042
00043 private Publisher<sensor_msgs.Imu> imuPublisher;
00044 private Publisher<jsk_gui_msgs.DeviceSensor> devPublisher;
00045 private Publisher<jsk_gui_msgs.Gravity> gravPublisher;
00046 private Publisher<jsk_gui_msgs.MagneticField> magPublisher;
00047
00048 @Override
00049 public GraphName getDefaultNodeName() {
00050 return GraphName.of("sensor_msgs/Imu");
00051 }
00052
00053 public AndroidSensorMessageNode(SensorManager manager,
00054 List<Sensor> sensors, AndroidSensorMessageView androidSensorMessageView,TextView textViewX,TextView textViewY,TextView textViewZ,int[] publishedSensor) {
00055 sensorManager = manager;
00056 this.sensors = sensors;
00057 this.androidSensorMessageView = androidSensorMessageView;
00058 this.textViewX = textViewX;
00059 this.textViewY = textViewY;
00060 this.textViewZ = textViewZ;
00061 this.publishedSensor = new int[13];
00062 for (int i = 0; i < 13; i++) {
00063
00064 this.publishedSensor[i] = publishedSensor[i];
00065 }
00066 }
00067
00068 public void changeSensor(int num) {
00069 publishedSensor[num] *= -1;
00070 setSensors();
00071 }
00072
00073 public void changeView(int num){
00074 currentView = num;
00075 }
00076
00077 @Override
00078 public void onStart(final ConnectedNode connectedNode) {
00079 try {
00080 imuPublisher = connectedNode.newPublisher("imu",
00081 "sensor_msgs/Imu");
00082 devPublisher = connectedNode.newPublisher("device",
00083 "jsk_gui_msgs/DeviceSensor");
00084 gravPublisher = connectedNode.newPublisher("gravity",
00085 "jsk_gui_msgs/Gravity");
00086 magPublisher = connectedNode.newPublisher("magneticfield",
00087 "jsk_gui_msgs/MagneticField");
00088
00089
00090 this.sensorListener = new SensorListener(imuPublisher, devPublisher,
00091 gravPublisher, magPublisher);
00092
00093 } catch (Exception e) {
00094
00095 }
00096
00097 }
00098
00099 public void setSensors() {
00100 for (Sensor sensor : sensors) {
00101 int sensorType = sensor.getType();
00102 switch (sensorType) {
00103
00104
00105
00106
00107
00108 case Sensor.TYPE_ACCELEROMETER:
00109 if (publishedSensor[sensorType - 1] == 1)
00110 sensorManager.registerListener(this.sensorListener,
00111 sensor, SensorManager.SENSOR_DELAY_FASTEST);
00112
00113 else
00114 sensorManager.unregisterListener(this.sensorListener,
00115 sensor);
00116
00117 break;
00118 case Sensor.TYPE_TEMPERATURE:
00119 if (publishedSensor[sensorType - 1] == 1)
00120 sensorManager.registerListener(this.sensorListener,
00121 sensor, SensorManager.SENSOR_DELAY_FASTEST);
00122 else
00123 sensorManager.unregisterListener(this.sensorListener,
00124 sensor);
00125
00126 break;
00127 case Sensor.TYPE_MAGNETIC_FIELD:
00128 if (publishedSensor[sensorType - 1] == 1)
00129 sensorManager.registerListener(this.sensorListener,
00130 sensor, SensorManager.SENSOR_DELAY_FASTEST);
00131 else
00132 sensorManager.unregisterListener(this.sensorListener,
00133 sensor);
00134
00135 break;
00136 case Sensor.TYPE_LIGHT:
00137 if (publishedSensor[sensorType - 1] == 1)
00138 sensorManager.registerListener(this.sensorListener,
00139 sensor, SensorManager.SENSOR_DELAY_FASTEST);
00140 else
00141 sensorManager.unregisterListener(this.sensorListener,
00142 sensor);
00143
00144 break;
00145 case Sensor.TYPE_PROXIMITY:
00146 if (publishedSensor[sensorType - 1] == 1)
00147 sensorManager.registerListener(this.sensorListener,
00148 sensor, SensorManager.SENSOR_DELAY_FASTEST);
00149 else
00150 sensorManager.unregisterListener(this.sensorListener,
00151 sensor);
00152
00153 break;
00154 case Sensor.TYPE_GYROSCOPE:
00155 if (publishedSensor[sensorType - 1] == 1)
00156 sensorManager.registerListener(this.sensorListener,
00157 sensor, SensorManager.SENSOR_DELAY_FASTEST);
00158 else
00159 sensorManager.unregisterListener(this.sensorListener,
00160 sensor);
00161
00162 break;
00163 case Sensor.TYPE_PRESSURE:
00164 if (publishedSensor[sensorType - 1] == 1)
00165 sensorManager.registerListener(this.sensorListener,
00166 sensor, SensorManager.SENSOR_DELAY_FASTEST);
00167 else
00168 sensorManager.unregisterListener(this.sensorListener,
00169 sensor);
00170 break;
00171 case Sensor.TYPE_GRAVITY:
00172 if (publishedSensor[sensorType - 1] == 1)
00173 sensorManager.registerListener(this.sensorListener,
00174 sensor, SensorManager.SENSOR_DELAY_FASTEST);
00175 else
00176 sensorManager.unregisterListener(this.sensorListener,
00177 sensor);
00178
00179 break;
00180 case Sensor.TYPE_ROTATION_VECTOR:
00181 if (publishedSensor[sensorType - 1] == 1)
00182 sensorManager.registerListener(this.sensorListener,
00183 sensor, SensorManager.SENSOR_DELAY_FASTEST);
00184 else
00185 sensorManager.unregisterListener(this.sensorListener,
00186 sensor);
00187
00188 break;
00189
00190
00191
00192
00193
00194 }
00195
00196 }
00197 }
00198
00199 @Override
00200 public void onShutdown(Node node) {
00201 sensorManager.unregisterListener(this.sensorListener);
00202 }
00203
00204 @Override
00205 public void onShutdownComplete(Node node) {
00206 }
00207
00208 @Override
00209 public void onError(Node node, Throwable throwable) {
00210 }
00211
00212 private class SensorListener implements SensorEventListener {
00213 Publisher<Imu> imuPublisher;
00214 Publisher<DeviceSensor> devPublisher;
00215 Publisher<Gravity> gravPublisher;
00216 Publisher<MagneticField> magPublisher;
00217
00218 sensor_msgs.Imu imuMessage;
00219 jsk_gui_msgs.DeviceSensor devMessage;
00220 jsk_gui_msgs.Gravity gravMessage;
00221 jsk_gui_msgs.MagneticField magMessage;
00222
00223 private SensorListener(Publisher<sensor_msgs.Imu> imuPublisher,
00224 Publisher<jsk_gui_msgs.DeviceSensor> devPublisher,
00225 Publisher<jsk_gui_msgs.Gravity> gravPublisher,
00226 Publisher<jsk_gui_msgs.MagneticField> magPublisher) {
00227
00228 this.imuPublisher = imuPublisher;
00229 this.devPublisher = devPublisher;
00230 this.gravPublisher = gravPublisher;
00231 this.magPublisher = magPublisher;
00232
00233 imuMessage = this.imuPublisher.newMessage();
00234 devMessage = this.devPublisher.newMessage();
00235 magMessage = this.magPublisher.newMessage();
00236 gravMessage = this.gravPublisher.newMessage();
00237 }
00238
00239 public void onSensorChanged(SensorEvent event) {
00240 long time_delta_millis;
00241 switch (event.sensor.getType()) {
00242 case Sensor.TYPE_ACCELEROMETER:
00243 if(currentView == Sensor.TYPE_ACCELEROMETER - 1){
00244 textViewX.setText("x:"+event.values[0]);
00245 textViewY.setText("y:"+event.values[1]);
00246 textViewZ.setText("z:"+event.values[2]);
00247 androidSensorMessageView.addValues(event.values);
00248 }
00249 imuMessage.getLinearAcceleration().setX(event.values[0]);
00250 imuMessage.getLinearAcceleration().setY(event.values[1]);
00251 imuMessage.getLinearAcceleration().setZ(event.values[2]);
00252 time_delta_millis = System.currentTimeMillis()
00253 - SystemClock.uptimeMillis();
00254 imuMessage.getHeader().setStamp(
00255 Time.fromMillis(time_delta_millis + event.timestamp
00256 / 1000000));
00257 imuMessage.getHeader().setFrameId("/imu");
00258 Log.v("type", "ok");
00259 imuPublisher.publish(imuMessage);
00260 break;
00261 case Sensor.TYPE_TEMPERATURE:
00262 if(currentView == Sensor.TYPE_TEMPERATURE - 1){
00263 textViewX.setText("x:"+event.values[0]);
00264 textViewY.setText("y:"+event.values[1]);
00265 textViewZ.setText("z:"+event.values[2]);
00266 androidSensorMessageView.addValues(event.values);
00267 }
00268 devMessage.setTemperature(event.values[0]);
00269 devPublisher.publish(devMessage);
00270 break;
00271 case Sensor.TYPE_MAGNETIC_FIELD:
00272 if(currentView == Sensor.TYPE_MAGNETIC_FIELD - 1){
00273 textViewX.setText("x:"+event.values[0]);
00274 textViewY.setText("y:"+event.values[1]);
00275 textViewZ.setText("z:"+event.values[2]);
00276
00277 androidSensorMessageView.addValues(event.values);
00278 }
00279 magMessage.getMagneticfield().setX(event.values[0]);
00280 magMessage.getMagneticfield().setY(event.values[1]);
00281 magMessage.getMagneticfield().setZ(event.values[2]);
00282
00283 time_delta_millis = System.currentTimeMillis()
00284 - SystemClock.uptimeMillis();
00285 magPublisher.publish(magMessage);
00286 break;
00287 case Sensor.TYPE_LIGHT:
00288 if(currentView == Sensor.TYPE_LIGHT - 1){
00289 textViewX.setText("x:"+event.values[0]);
00290 textViewY.setText("y:"+event.values[1]);
00291 textViewZ.setText("z:"+event.values[2]);
00292
00293 androidSensorMessageView.addValues(event.values);
00294 }
00295 devMessage.setLight(event.values[0]);
00296 devPublisher.publish(devMessage);
00297 break;
00298 case Sensor.TYPE_PROXIMITY:
00299 if(currentView == Sensor.TYPE_PROXIMITY - 1){
00300 textViewX.setText("x:"+event.values[0]);
00301 textViewY.setText("y:"+event.values[1]);
00302 textViewZ.setText("z:"+event.values[2]);
00303
00304 androidSensorMessageView.addValues(event.values);
00305 }
00306 devMessage.setProximity(event.values[0]);
00307 devPublisher.publish(devMessage);
00308 break;
00309 case Sensor.TYPE_GYROSCOPE:
00310 if(currentView == Sensor.TYPE_GYROSCOPE - 1){
00311 textViewX.setText("x:"+event.values[0]);
00312 textViewY.setText("y:"+event.values[1]);
00313 textViewZ.setText("z:"+event.values[2]);
00314
00315 androidSensorMessageView.addValues(event.values);
00316 }
00317 imuMessage.getAngularVelocity().setX(event.values[0]);
00318 imuMessage.getAngularVelocity().setY(event.values[1]);
00319 imuMessage.getAngularVelocity().setZ(event.values[2]);
00320 time_delta_millis = System.currentTimeMillis()
00321 - SystemClock.uptimeMillis();
00322 imuMessage.getHeader().setStamp(
00323 Time.fromMillis(time_delta_millis + event.timestamp
00324 / 1000000));
00325 imuMessage.getHeader().setFrameId("/imu");
00326 imuPublisher.publish(imuMessage);
00327 break;
00328 case Sensor.TYPE_PRESSURE:
00329 if(currentView == Sensor.TYPE_PRESSURE - 1){
00330 textViewX.setText("x:"+event.values[0]);
00331 textViewY.setText("y:"+event.values[1]);
00332 textViewZ.setText("z:"+event.values[2]);
00333
00334 androidSensorMessageView.addValues(event.values);
00335 }
00336 devMessage.setPressure(event.values[0]);
00337 devPublisher.publish(devMessage);
00338 break;
00339 case Sensor.TYPE_GRAVITY:
00340 if(currentView == Sensor.TYPE_GRAVITY - 1){
00341 textViewX.setText("x:"+event.values[0]);
00342 textViewY.setText("y:"+event.values[1]);
00343 textViewZ.setText("z:"+event.values[2]);
00344
00345 androidSensorMessageView.addValues(event.values);
00346 }
00347 gravMessage.getGravity().setX(event.values[0]);
00348 gravMessage.getGravity().setY(event.values[1]);
00349 gravMessage.getGravity().setZ(event.values[2]);
00350 gravPublisher.publish(gravMessage);
00351 break;
00352 case Sensor.TYPE_ROTATION_VECTOR:
00353 if(currentView == Sensor.TYPE_ROTATION_VECTOR - 1){
00354 textViewX.setText("x:"+event.values[0]);
00355 textViewY.setText("y:"+event.values[1]);
00356 textViewZ.setText("z:"+event.values[2]);
00357
00358 androidSensorMessageView.addValues(event.values);
00359 }
00360 float[] quaternion = new float[4];
00361 SensorManager.getQuaternionFromVector(quaternion, event.values);
00362 imuMessage.getOrientation().setX(quaternion[0]);
00363 imuMessage.getOrientation().setY(quaternion[1]);
00364 imuMessage.getOrientation().setZ(quaternion[2]);
00365 imuMessage.getOrientation().setW(quaternion[3]);
00366 imuMessage.getHeader().setFrameId("/imu");
00367 imuPublisher.publish(imuMessage);
00368 break;
00369
00370
00371
00372
00373
00374
00375 }
00376
00377 }
00378
00379 public void onAccuracyChanged(Sensor sensor, int accuracy) {
00380 }
00381
00382 }
00383
00384 }