Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 package eu.intermodalics.tango_ros_streamer.nodes;
00018
00019 import android.app.Activity;
00020 import android.content.Context;
00021 import android.hardware.Sensor;
00022 import android.hardware.SensorEvent;
00023 import android.hardware.SensorEventListener;
00024 import android.hardware.SensorManager;
00025
00026 import org.ros.namespace.GraphName;
00027 import org.ros.node.AbstractNodeMain;
00028 import org.ros.node.ConnectedNode;
00029 import org.ros.node.NodeMain;
00030 import org.ros.node.topic.Publisher;
00031
00032 import eu.intermodalics.tango_ros_common.ConnectedNodeLogger;
00033 import sensor_msgs.Imu;
00034
00038 public class ImuNode extends AbstractNodeMain implements NodeMain, SensorEventListener {
00039 private static final String TAG = ImuNode.class.getSimpleName();
00040 private static final String NODE_NAME = "android";
00041
00042 private ConnectedNode mConnectedNode;
00043 private Publisher<Imu> mImuPublisher;
00044 private Imu mImuMessage;
00045 private ConnectedNodeLogger mLog;
00046
00047 private SensorManager mSensorManager;
00048 private Sensor mRotationSensor;
00049 private Sensor mGyroscopeSensor;
00050 private Sensor mAccelerometerSensor;
00051 private boolean mNewRotationData = false;
00052 private boolean mNewGyroscopeData = false;
00053 private boolean mNewAccelerometerData = false;
00054
00055 public ImuNode(Activity activity) {
00056 mSensorManager = (SensorManager) activity.getSystemService(Context.SENSOR_SERVICE);
00057 mRotationSensor = mSensorManager.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR);
00058 mGyroscopeSensor = mSensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE);
00059 mAccelerometerSensor = mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
00060 }
00061
00062 public GraphName getDefaultNodeName() { return GraphName.of(NODE_NAME); }
00063
00064 public void onStart(ConnectedNode connectedNode) {
00065 mConnectedNode = connectedNode;
00066 mLog = new ConnectedNodeLogger(connectedNode);
00067 mImuPublisher = connectedNode.newPublisher("android/imu", Imu._TYPE);
00068 mImuMessage = mConnectedNode.getTopicMessageFactory().newFromType(Imu._TYPE);
00069
00070 mSensorManager.registerListener(this, mRotationSensor, SensorManager.SENSOR_DELAY_FASTEST);
00071 mSensorManager.registerListener(this, mGyroscopeSensor, SensorManager.SENSOR_DELAY_FASTEST);
00072 mSensorManager.registerListener(this, mAccelerometerSensor, SensorManager.SENSOR_DELAY_FASTEST);
00073 }
00074
00075 @Override
00076 public void onAccuracyChanged(Sensor sensor, int accuracy) {
00077
00078 }
00079
00080 @Override
00081 public void onSensorChanged(SensorEvent event) {
00082 switch (event.sensor.getType()) {
00083 case Sensor.TYPE_ROTATION_VECTOR:
00084 mNewRotationData = true;
00085 float[] quaternion = new float[4];
00086 SensorManager.getQuaternionFromVector(quaternion, event.values);
00087 mImuMessage.getOrientation().setW(quaternion[0]);
00088 mImuMessage.getOrientation().setX(quaternion[1]);
00089 mImuMessage.getOrientation().setY(quaternion[2]);
00090 mImuMessage.getOrientation().setZ(quaternion[3]);
00091 mImuMessage.setOrientationCovariance(new double[]{0, 0, 0, 0, 0, 0, 0, 0, 0});
00092 break;
00093 case Sensor.TYPE_GYROSCOPE:
00094 mNewGyroscopeData = true;
00095 mImuMessage.getAngularVelocity().setX(event.values[0]);
00096 mImuMessage.getAngularVelocity().setY(event.values[1]);
00097 mImuMessage.getAngularVelocity().setZ(event.values[2]);
00098 mImuMessage.setAngularVelocityCovariance(new double[]{0, 0, 0, 0, 0, 0, 0, 0, 0});
00099 break;
00100 case Sensor.TYPE_ACCELEROMETER:
00101 mNewAccelerometerData = true;
00102 mImuMessage.getLinearAcceleration().setX(event.values[0]);
00103 mImuMessage.getLinearAcceleration().setY(event.values[1]);
00104 mImuMessage.getLinearAcceleration().setZ(event.values[2]);
00105 mImuMessage.setLinearAccelerationCovariance(new double[]{0, 0, 0, 0, 0, 0, 0, 0, 0});
00106 break;
00107 default:
00108 break;
00109 }
00110 if (mNewRotationData && mNewGyroscopeData && mNewAccelerometerData) {
00111 try {
00112 mImuMessage.getHeader().setStamp(mConnectedNode.getCurrentTime());
00113 mImuMessage.getHeader().setFrameId("imu");
00114 mImuPublisher.publish(mImuMessage);
00115 } catch (NullPointerException e) {
00116 mLog.error(TAG, "Could not get current time. If you are using simulated time, " +
00117 "/clock might not be published yet.");
00118 e.printStackTrace();
00119 }
00120 mNewRotationData = false;
00121 mNewGyroscopeData = false;
00122 mNewAccelerometerData = false;
00123 }
00124 }
00125 }