Merge pull request Intermodalics#144 from Intermodalics/feature/publi…

…sh_imu

Feature/publish imu

TangoRosStreamer/app/src/main/java/eu/intermodalics/tango_ros_streamer/ImuNode.java

@@ -0,0 +1,121 @@
+/*
+ * Copyright 2017 Intermodalics All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+package eu.intermodalics.tango_ros_streamer;
+
+import android.app.Activity;
+import android.content.Context;
+import android.hardware.Sensor;
+import android.hardware.SensorEvent;
+import android.hardware.SensorEventListener;
+import android.hardware.SensorManager;
+import android.os.SystemClock;
+
+import org.apache.commons.logging.Log;
+import org.ros.message.Time;
+import org.ros.namespace.GraphName;
+import org.ros.node.AbstractNodeMain;
+import org.ros.node.ConnectedNode;
+import org.ros.node.NodeMain;
+import org.ros.node.topic.Publisher;
+
+import sensor_msgs.Imu;
+
+/**
+ *
+ */
+public class ImuNode extends AbstractNodeMain implements NodeMain, SensorEventListener {
+    private static final String NODE_NAME = "android";
+
+    private ConnectedNode mConnectedNode;
+    private Publisher<Imu> mImuPublisher;
+    private Imu mImuMessage;
+    private Log mLog;
+
+    private SensorManager mSensorManager;
+    private Sensor mRotationSensor;
+    private Sensor mGyroscopeSensor;
+    private Sensor mAccelerometerSensor;
+    private boolean mNewRotationData = false;
+    private boolean mNewGyroscopeData = false;
+    private boolean mNewAccelerometerData = false;
+
+    public ImuNode(Activity activity) {
+        mSensorManager = (SensorManager) activity.getSystemService(Context.SENSOR_SERVICE);
+        mRotationSensor = mSensorManager.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR);
+        mGyroscopeSensor = mSensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE);
+        mAccelerometerSensor = mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
+    }
+
+    public GraphName getDefaultNodeName() { return GraphName.of(NODE_NAME); }
+
+    public void onStart(ConnectedNode connectedNode) {
+        mConnectedNode = connectedNode;
+        mLog = connectedNode.getLog();
+        mImuPublisher = connectedNode.newPublisher("android/imu", Imu._TYPE);
+        mImuMessage = mConnectedNode.getTopicMessageFactory().newFromType(Imu._TYPE);
+
+        mSensorManager.registerListener(this, mRotationSensor, SensorManager.SENSOR_DELAY_FASTEST);
+        mSensorManager.registerListener(this, mGyroscopeSensor, SensorManager.SENSOR_DELAY_FASTEST);
+        mSensorManager.registerListener(this, mAccelerometerSensor, SensorManager.SENSOR_DELAY_FASTEST);
+    }
+
+    @Override
+	public void onAccuracyChanged(Sensor sensor, int accuracy) {
+        // Not used.
+	}
+
+    @Override
+    public void onSensorChanged(SensorEvent event) {
+        switch (event.sensor.getType()) {
+            case Sensor.TYPE_ROTATION_VECTOR:
+                mNewRotationData = true;
+                float[] quaternion = new float[4];
+                SensorManager.getQuaternionFromVector(quaternion, event.values);
+                mImuMessage.getOrientation().setW(quaternion[0]);
+                mImuMessage.getOrientation().setX(quaternion[1]);
+                mImuMessage.getOrientation().setY(quaternion[2]);
+                mImuMessage.getOrientation().setZ(quaternion[3]);
+                mImuMessage.setOrientationCovariance(new double[]{0, 0, 0, 0, 0, 0, 0, 0, 0});
+                break;
+            case Sensor.TYPE_GYROSCOPE:
+                mNewGyroscopeData = true;
+                mImuMessage.getAngularVelocity().setX(event.values[0]);
+                mImuMessage.getAngularVelocity().setY(event.values[1]);
+                mImuMessage.getAngularVelocity().setZ(event.values[2]);
+                mImuMessage.setAngularVelocityCovariance(new double[]{0, 0, 0, 0, 0, 0, 0, 0, 0});
+                break;
+            case Sensor.TYPE_ACCELEROMETER:
+                mNewAccelerometerData = true;
+                mImuMessage.getLinearAcceleration().setX(event.values[0]);
+                mImuMessage.getLinearAcceleration().setY(event.values[1]);
+                mImuMessage.getLinearAcceleration().setZ(event.values[2]);
+                mImuMessage.setLinearAccelerationCovariance(new double[]{0, 0, 0, 0, 0, 0, 0, 0, 0});
+                break;
+            default:
+                break;
+        }
+        if (mNewRotationData && mNewGyroscopeData && mNewAccelerometerData) {
+            long timeOffset = System.currentTimeMillis() - SystemClock.uptimeMillis();
+            mImuMessage.getHeader().setStamp(Time.fromMillis(timeOffset + event.timestamp / 1000000));
+            mImuMessage.getHeader().setFrameId("imu");
+            mImuPublisher.publish(mImuMessage);
+            mNewRotationData = false;
+            mNewGyroscopeData = false;
+            mNewAccelerometerData = false;
+        }
+    }
+}

TangoRosStreamer/app/src/main/java/eu/intermodalics/tango_ros_streamer/RunningActivity.java

@@ -79,6 +79,7 @@ enum TangoStatus {
     private boolean mRunLocalMaster = false;
     private String mMasterUri = "";
     private ParameterNode mParameterNode;
+    private ImuNode mImuNode;
     private RosStatus mRosStatus = RosStatus.MASTER_NOT_CONNECTED;
     private TangoStatus mTangoStatus = TangoStatus.SERVICE_NOT_CONNECTED;
     private Logger mLogger;
@@ -328,7 +329,10 @@ protected void init(NodeMainExecutor nodeMainExecutor) {
         mParameterNode = new ParameterNode(this, dynamicParams, tangoConfigurationParameters);
         nodeConfiguration.setNodeName(mParameterNode.getDefaultNodeName());
         nodeMainExecutor.execute(mParameterNode, nodeConfiguration);
-
+        // Create node publishing IMU data.
+        mImuNode = new ImuNode(this);
+        nodeConfiguration.setNodeName(mImuNode.getDefaultNodeName());
+        nodeMainExecutor.execute(mImuNode, nodeConfiguration);
         // Create and start Tango ROS Node
         nodeConfiguration.setNodeName(TangoRosNode.NODE_NAME);
         if (TangoInitializationHelper.loadTangoSharedLibrary() !=

tango_ros_common/tango_ros_native/src/tango_ros_node.cpp

@@ -419,6 +419,17 @@ void TangoRosNode::TangoDisconnect() {
 void TangoRosNode::PublishStaticTransforms() {
   TangoCoordinateFramePair pair;
   TangoPoseData pose;
+
+  pair.base = TANGO_COORDINATE_FRAME_DEVICE;
+  pair.target = TANGO_COORDINATE_FRAME_IMU;
+  TangoService_getPoseAtTime(0.0, pair, &pose);
+  geometry_msgs::TransformStamped device_T_imu;
+  toTransformStamped(pose, time_offset_, &device_T_imu);
+  device_T_imu.header.frame_id = toFrameId(TANGO_COORDINATE_FRAME_DEVICE);
+  device_T_imu.child_frame_id = toFrameId(TANGO_COORDINATE_FRAME_IMU);
+  device_T_imu.header.stamp = ros::Time::now();
+  tf_static_broadcaster_.sendTransform(device_T_imu);
+
   if (publisher_config_.publish_point_cloud || publisher_config_.publish_laser_scan) {
     pair.base = TANGO_COORDINATE_FRAME_DEVICE;
     pair.target = TANGO_COORDINATE_FRAME_CAMERA_DEPTH;