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