Skip to content
Snippets Groups Projects
Commit 34780f74 authored by Hailong Zhu's avatar Hailong Zhu
Browse files

-added Odometry.java

parent 9f21db10
No related branches found
No related tags found
No related merge requests found
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) {
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment