From 34780f74df479d9d39dd175dca43074f7b3efc7c Mon Sep 17 00:00:00 2001 From: Hailong <hailong.zhu@tu-dresden.de> Date: Thu, 21 Oct 2021 11:17:51 +0200 Subject: [PATCH] -added Odometry.java --- .../ipos/project/Functionality/Odometry.java | 90 ++++++++++++++++++- 1 file changed, 87 insertions(+), 3 deletions(-) diff --git a/src/main/java/ipos/project/Functionality/Odometry.java b/src/main/java/ipos/project/Functionality/Odometry.java index 876404b..aef3edc 100644 --- a/src/main/java/ipos/project/Functionality/Odometry.java +++ b/src/main/java/ipos/project/Functionality/Odometry.java @@ -1,15 +1,99 @@ package ipos.project.Functionality; -import ipos.models.SimpleScene.IposPosition; +import ipos.project.DataModellntegration.iPos_Datamodel.*; import org.slf4j.Logger; import org.slf4j.LoggerFactory; -import org.springframework.jms.annotation.JmsListener; import org.springframework.stereotype.Component; +import java.sql.Timestamp; +import java.text.ParseException; +import java.text.SimpleDateFormat; +import java.util.Date; +import java.util.MissingResourceException; + @Component public class Odometry { - private final Logger LOG = LoggerFactory.getLogger(getClass()); + private final String agentId; + private double accuracy; + private double timeOfLastCalibration; + private double timeLastSensorEvent; + private double [] a = new double[]{0, 0, 0}; + private double [] aOffset; + private double [] v = new double[]{0, 0, 0}; + private double [] p = new double[]{0, 0, 0}; + private double [] delta = new double[]{0, 0, 0}; + private double [] vLast = new double[]{0, 0, 0}; + private double [] pLast = new double[]{0, 0, 0}; + private double NS2S = 1/10000.0; + private boolean calibrated = false; + SimpleDateFormat sdf2 = new SimpleDateFormat("yyyy-MM-dd hh:mm:ss.SSS"); + + public Odometry(String agentId) { + this.agentId = agentId; + } + + public void calibrate (PositionEvent event) throws ParseException { + this.vLast[0] = 0; + this.vLast[1] = 0; + this.vLast[2] = 0; + aOffset =a; + this.accuracy = 0.03; + if (event.getPlacing().getPosition().getPoint() instanceof Point3D) { + this.pLast[0] = ((Point3D) event.getPlacing().getPosition().getPoint()).getX(); + this.pLast[1] = ((Point3D) event.getPlacing().getPosition().getPoint()).getY(); + this.pLast[2] = ((Point3D) event.getPlacing().getPosition().getPoint()).getZ(); + } + Date date = sdf2.parse(event.getTimeStamp()); + timeOfLastCalibration = date.getTime(); + } + + public PositionEvent Update (IMU imuRawdataEvent) throws ParseException { + IPos_DatamodelFactory datamodelFactory = IPos_DatamodelFactory.eINSTANCE; + Timestamp timestamp = new Timestamp(System.currentTimeMillis()); + + + + Date date = sdf2.parse(imuRawdataEvent.getTimeStamp()); + timeLastSensorEvent = date.getTime(); + if (calibrated) { + double dt = (date.getTime() - timeLastSensorEvent) * NS2S; + a[0] = imuRawdataEvent.getAcceleration().getX() - aOffset[0]; + a[1] = imuRawdataEvent.getAcceleration().getY() - aOffset[1]; + a[2] = imuRawdataEvent.getAcceleration().getZ() - aOffset[2]; + for (int index = 0; index < 3; ++index) { + double last_velocity = vLast[index]; + v[index] += (a[index] + vLast[index]) / 2 * dt; + delta[index] += (v[index] + last_velocity) / 2 * dt; + p[index] = pLast[index] + delta[index]; + } + accuracy = (date.getTime() - timeOfLastCalibration) * 0.01; + + PositionEvent calPositionEvent = datamodelFactory.createPositionEvent(); + Placing calPlacing = datamodelFactory.createPlacing(); + LocalizableObject calLObject = datamodelFactory.createLocalizableObject(); + Position calPosition = datamodelFactory.createPosition(); + Point3D calPoint3D = datamodelFactory.createPoint3D(); + Gaussian calGaussian = datamodelFactory.createGaussian(); + calGaussian.setConfidenceInterval((float) accuracy); + calPoint3D.setX((float) p[0]); + calPoint3D.setY((float) p[1]); + calPoint3D.setZ((float) p[2]); + calPosition.setPoint(calPoint3D); + calPosition.setAccuracy(calGaussian); + calPlacing.setPosition(calPosition); + calLObject.setId(imuRawdataEvent.getSensorId()); + calPositionEvent.setTimeStamp(imuRawdataEvent.getTimeStamp()); + calPositionEvent.setPlacing(calPlacing); + return calPositionEvent; + } + else { + a[0] = imuRawdataEvent.getAcceleration().getX(); + a[1] = imuRawdataEvent.getAcceleration().getY(); + a[2] = imuRawdataEvent.getAcceleration().getZ(); + throw new MissingResourceException("Odometry not calibrated!", "Odometry", "calibration"); + } + } //TODO // @JmsListener(destination = "/positions123", containerFactory = "myFactory") // public void receiveMessage(IposPosition pos) { -- GitLab