@Override public synchronized void onNewMessage(sensor_msgs.Imu message) { this.timeStamp = message.getHeader().getStamp().totalNsecs(); this.seq_id = message.getHeader().getSeq(); this.frameId = message.getHeader().getFrameId(); RosTools.packRosQuaternionToQuat4d(message.getOrientation(), this.orientationEstimate); RosTools.packRosVector3ToVector3d(message.getAngularVelocity(), this.angularVelocity); RosTools.packRosVector3ToVector3d(message.getLinearAcceleration(), this.linearAcceleration); onNewMessage(timeStamp, seq_id, orientationEstimate, angularVelocity, linearAcceleration); }
counter++; message.setHeader(header); message.setLinearAcceleration(localLinearAcceleration); message.setOrientation(localOrientation); message.setAngularVelocity(localAngularVelocity);
counter++; message.setHeader(header); message.setLinearAcceleration(localLinearAcceleration); message.setOrientation(localOrientation); message.setAngularVelocity(localAngularVelocity);
@Override public synchronized void onNewMessage(sensor_msgs.Imu message) { this.timeStamp = message.getHeader().getStamp().totalNsecs(); this.seq_id = message.getHeader().getSeq(); this.frameId = message.getHeader().getFrameId(); RosTools.packRosQuaternionToEuclidQuaternion(message.getOrientation(), this.orientationEstimate); RosTools.packRosVector3ToEuclidTuple3D(message.getAngularVelocity(), this.angularVelocity); RosTools.packRosVector3ToEuclidTuple3D(message.getLinearAcceleration(), this.linearAcceleration); onNewMessage(timeStamp, seq_id, orientationEstimate, angularVelocity, linearAcceleration); }