Skip to content
Snippets Groups Projects
Commit 138cfa61 authored by FrankR's avatar FrankR
Browse files

Statusupdate, Demonstrator in Arbeit

parent 9f21db10
No related branches found
No related tags found
No related merge requests found
Showing
with 268 additions and 43 deletions
{"frames": [{"id": "cobot1_door_zone", "space": [{"position": {"refSystemId": "ROOT", "point": {"x": 2.0,"y": 0.0,"z": 0.0}, "accuracy": 0.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "x": 4.0, "y": 4.0, "z":6.0}]}, {"id": "cobot1_window_zone", "space": [{"position": {"refSystemId": "ROOT", "point": {"x": 2.0,"y": 4.0,"z": 0.0}, "accuracy": 0.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "x": 4.0, "y": 4.0, "z":6.0}]}, {"id": "robolab_east", "space": [{"position": {"refSystemId": "ROOT", "point": {"x": 4.0,"y": 2.0,"z": 0.0}, "accuracy": 0.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "x": 8.0, "y": 10.0, "z":6.0}]}, {"id": "robolab_west", "space": [{"position": {"refSystemId": "ROOT", "point": {"x": -4.0,"y": 2.0,"z": 0.0}, "accuracy": 0.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "x": 8.0, "y": 10.0, "z":6.0}]} ]}
{"frames": [{"id": "cobot1_door_zone", "space": [{"position": {"refSystemId": "ROOT", "point": {"x": 2.0,"y": 0.0,"z": 0.0}, "accuracy": 0.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "x": 4.0, "y": 4.0, "z":6.0}]}, {"id": "cobot1_window_zone", "space": [{"position": {"refSystemId": "ROOT", "point": {"x": 2.0,"y": 4.0,"z": 0.0}, "accuracy": 0.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "x": 4.0, "y": 4.0, "z":6.0}]}, {"id": "robolab_east", "space": [{"position": {"refSystemId": "ROOT", "point": {"x": 4.0,"y": 2.0,"z": 0.0}, "accuracy": 0.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "x": 8000000.0, "y": 100000000.0, "z":6000000.0}]}, {"id": "robolab_west", "space": [{"position": {"refSystemId": "ROOT", "point": {"x": -4.0,"y": 2.0,"z": 0.0}, "accuracy": 0.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "x": 8.0, "y": 10.0, "z":6.0}]} ]}
{"refSystems": [{"id": "ROOT"}, {"id": "CETI_ROOT", "position": {"refSystemId": "ROOT", "point": {"x": 3.05,"y": 2.08,"z": 0.0}, "accuracy": 1.0}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0}}]}
{"objectConfigs": [{"agentId": "turtlebot", "sensorId": "UWB_1", "agentType": "ROBOT", "sensorType": "UWB"}, {"agentId": "turtlebot", "sensorId": "UWB_2", "agentType": "ROBOT", "sensorType": "UWB"}, {"agentId": "turtlebot", "sensorId": "98:CD:AC:26:2D:18", "agentType": "ROBOT", "sensorType": "RFID_SCANNER"}]}
{"objectConfigs": [{"agentId": "turtlebot", "sensorId": "UWB_2", "agentType": "ROBOT", "sensorType": "UWB"}, {"agentId": "turtlebot", "sensorId": "UWB_1", "agentType": "ROBOT", "sensorType": "UWB"}, {"agentId": "turtlebot", "sensorId": "98:CD:AC:26:2D:18", "agentType": "ROBOT", "sensorType": "RFID_SCANNER"}, {"agentId": "turtlebot", "sensorId": "handy1", "agentType": "ROBOT", "sensorType": "IMU"}]}
{"pois": [{"id": "UWB_BEACON_1", "description": "Beacon_Position", "data": {"type" : "UWB"}, "position": {"refSystemId": "ROOT", "point": {"x": 3.1,"y": 0.5,"z": 0.0}, "accuracy": 1.0}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0} }, {"id": "UWB_BEACON_2", "description": "Beacon_Position", "data": {"type" : "UWB"}, "position": {"refSystemId": "ROOT", "point": {"x": -0.3,"y": 4.5,"z": 0.0}, "accuracy": 1.0}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0}}, {"id": "83221710", "description": "RFID_TAG", "data": {"type" : "NFC"}, "position": {"refSystemId": "ROOT", "point": {"x": 2.1,"y": 0.4,"z": 0.5}, "accuracy": 0.1}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0} }, {"id": "31762128", "description": "RFID_TAG", "data": {"type" : "NFC"}, "position": {"refSystemId": "ROOT", "point": {"x": 2.2,"y": 3.4,"z": 0.5}, "accuracy": 0.1}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0} }]}
{"monitoringRequests": [{"frameIds": ["cobot1_door_zone", "cobot1_window_zone"], "monitoringTaskId": "RobolabMonitoringCeti", "serializationType": "protobuf"}, {"frameIds": ["cobot1_door_zone", "cobot1_window_zone", "robolab_east", "robolab_west"], "monitoringTaskId": "RobolabMonitoringFrontend", "serializationType": "json"}]}
\ No newline at end of file
{"frames": [{"id": "robolab", "space": [{"position": {"refSystemId": "ROOT", "point": {"x": 2.0,"y": 0.0,"z": 0.0}, "accuracy": 0.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "x": 10.0, "y": 10.0, "z":10.0}]} ]}
{"refSystems": [{"id": "ROOT"}]}
{"objectConfigs": [{"agentId": "turtlebot", "sensorId": "UWB_1", "agentType": "HUMAN", "sensorType": "UWB"}, {"agentId": "turtlebot", "sensorId": "98:CD:AC:26:2D:18", "agentType": "ROBOT", "sensorType": "RFID_SCANNER"}]}
{"objectConfigs": [{"agentId": "turtlebot", "sensorId": "UWB_1", "agentType": "ROBOT", "sensorType": "UWB"}, {"agentId": "turtlebot", "sensorId": "98:CD:AC:26:2D:18", "agentType": "ROBOT", "sensorType": "RFID_SCANNER"}]}
{"pois": [{"id": "83221710", "description": "RFID_TAG", "data": {"type" : "NFC"}, "position": {"refSystemId": "ROOT", "point": {"x": 2.1,"y": 0.4,"z": 0.5}, "accuracy": 0.1}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0} }, {"id": "31762128", "description": "RFID_TAG", "data": {"type" : "NFC"}, "position": {"refSystemId": "ROOT", "point": {"x": 2.2,"y": 3.4,"z": 0.5}, "accuracy": 0.1}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0} }]}
{"monitoringRequests": [{"frameId": "robolab", "monitoringTaskId": "ipos/client/position", "serializationType": "json"}]}
\ No newline at end of file
......@@ -12,12 +12,18 @@ public class CustomLoggingFilter extends Filter<ILoggingEvent> {
return FilterReply.DENY;
} else if (event.getMessage().contains("SDF:")) {
return FilterReply.DENY;
} else if (event.getMessage().contains("SDF-DEBUG:")) {
return FilterReply.DENY;
} else if (event.getMessage().contains("SDF-PUB:")) {
return FilterReply.DENY;
} else if (event.getMessage().contains("INDFRO:")){
return FilterReply.ACCEPT;
return FilterReply.DENY;
} else if (event.getMessage().contains("INDFRO-DEBUG:")){
return FilterReply.DENY;
} else {
} else if (event.getMessage().contains("ODO:")){
return FilterReply.DENY;
} else {
return FilterReply.ACCEPT;
}
}
......
package ipos.project.DataModellntegration.SimpleSceneIntegration;
import com.fasterxml.jackson.core.JsonProcessingException;
import com.google.protobuf.InvalidProtocolBufferException;
import ipos.models.SimpleScene;
import ipos.project.DataModellntegration.SimpleSceneIntegration.api.MqttRequestHandler;
import ipos.project.DataModellntegration.SimpleSceneIntegration.service.SimpleSceneTransformer;
import ipos.project.DataModellntegration.SimpleSceneIntegration.service.impl.ExternalPubServiceImpl;
import ipos.project.DataModellntegration.iPos_Datamodel.DataStorageQueryResponse;
import ipos.project.DataModellntegration.iPos_Datamodel.Point3D;
import ipos.project.DataModellntegration.iPos_Datamodel.PositionEvent;
import ipos.project.DataModellntegration.iPos_Datamodel.ZoneDescriptor;
import ipos.project.DataModellntegration.iPos_Datamodel.*;
import ipos.project.DataModellntegration.iPos_Datamodel.impl.PositionEventImpl;
import ipos.project.UseCaseController.Administration;
import ipos.project.UseCaseController.PositionMonitoring;
import ipos.project.mapper.ProtoJsonMap;
......@@ -17,6 +16,7 @@ import org.eclipse.emf.common.util.EList;
import org.eclipse.paho.client.mqttv3.MqttMessage;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.stereotype.Component;
import com.fasterxml.jackson.databind.ObjectMapper;
import java.io.BufferedReader;
import java.io.File;
......@@ -102,7 +102,8 @@ public class SimpleSceneIntegration {
// verwenden: DataServices.create-Methoden verwenden. Für ZoneDeskriptoren neue create-Funktionen schreiben
// PositionEvent posEvent_copy = duplicatePositionEvent(positionEvent);
SimpleScene.IposPositionEvent posEvent_proto = SimpleSceneTransformer.posEvent_internal2Proto(scale_position_frontend_app(positionEvent, 15), notificationType);
// scale_position_frontend_app(posEvent_copy, 15);
SimpleScene.IposPositionEvent posEvent_proto = SimpleSceneTransformer.posEvent_internal2Proto(positionEvent, notificationType);
MqttMessage mqttMessage = mqttService.createMqttMsg(posEvent_proto, 0, false);
logging_sdf(monitoringTaskId, serializationType, posEvent_proto);
logging_indfro(positionEvent, monitoringTaskId, serializationType);
......@@ -114,6 +115,18 @@ public class SimpleSceneIntegration {
}
}
private static PositionEvent duplicatePositionEvent(PositionEvent positionEvent) {
PositionEventImpl posEvent_copy = (PositionEventImpl) positionEvent;
ObjectMapper objectMapper = new ObjectMapper();
try {
posEvent_copy = objectMapper.readValue(objectMapper.writeValueAsString(positionEvent), PositionEventImpl.class);
} catch (JsonProcessingException e) {
LOG.warn("could not duplicate PositionEvent");
e.printStackTrace();
}
return posEvent_copy;
}
private static void logging_indfro(PositionEvent positionEvent, String monitoringTaskId, String serializationType) {
LOG.info("INDFRO: Publishing PositionEvent on topic " + monitoringTaskId + ": ZoneDescriptors:" + logZoneDescr(positionEvent.getZonedescriptors()) + "; serializationType: " + serializationType);
}
......@@ -137,8 +150,9 @@ public class SimpleSceneIntegration {
// publishes JSON over MQTT
if (PositionMonitoring.JSON_SERIALIZATION_TYPE.equals(serializationType)){
mqttService.publish(topic, jsonString, 0, false);
LOG.info("publishing JSON mqttMessage:" + jsonString + " on topic: " + topic);
String fakeTopic = "ipos/client/position";
mqttService.publish(fakeTopic, jsonString, 0, false);
LOG.info("publishing JSON mqttMessage:" + jsonString + " on topic: " + fakeTopic);
}
}
......@@ -151,17 +165,28 @@ public class SimpleSceneIntegration {
}
private static void logging_sdf(String monitoringTaskId, String serializationType, SimpleScene.IposPositionEvent posEvent_proto) {
for (SimpleScene.IposObject obj : posEvent_proto.getObjectList()){
for (SimpleScene.IposObject obj : posEvent_proto.getObjectsList()){
String agentId = obj.getId();
String sensorId = obj.getSensorId();
String sensorType = obj.getSensorType();
String timestamp = obj.getLastPosUpdate();
String topic = monitoringTaskId;
LOG.info("SDF: Publishing: sensorType: " + sensorType + "; timestamp: " + timestamp + "; sensorId: " + sensorId + "; agentId: " + agentId + "; topic: " + monitoringTaskId + "; serializationType: " + serializationType);
float accuracy = obj.getPosition().getAccuracy();
String positionString = toPositionString(obj.getPosition());
LOG.info("SDF-PUB: Publishing: sensorType: " + sensorType + "; timestamp: " + timestamp + "; accuracy: " + accuracy + "position: " + positionString + "; sensorId: " + sensorId + "; agentId: " + agentId + "; topic: " + monitoringTaskId + "; serializationType: " + serializationType);
LOG.info("SDF: ");
LOG.info("SDF: ");
LOG.info("SDF: ");
}
}
private static String toPositionString(SimpleScene.IposPosition position) {
String posString = "(";
posString += "x: " + position.getPoint().getX();
posString += "y: " + position.getPoint().getY();
posString += "z: " + position.getPoint().getZ();
posString += ")";
return posString;
}
}
......@@ -83,7 +83,7 @@ public class SimpleSceneTransformer {
if (!PositionMonitoring.UNDEFINED_TYPE.equals(notificationType)) { // if undefined, protobuf-object will be created without explicit notificationtype-information
protoIposPosEvent.setType(notificationType);
}
protoIposPosEvent.addObject(protoIposObject);
protoIposPosEvent.addObjects(protoIposObject);
return protoIposPosEvent;
}
......
......@@ -32,6 +32,10 @@ public class DataServices {
}
public static LocalizableObject getLObjectByIdOrNull(String lObjectId) {
if(wma == null){
LOG.info("wma is null. lObjectId: " + lObjectId);
return null;
}
for (Agent agent : wma.getAllAgents()){
for (LocalizableObject lObject : agent.getLObject()){
if (lObject.getId().equals(lObjectId)){
......
package ipos.project.Functionality;
import ipos.models.SimpleScene.IposPosition;
import ipos.project.DataModellntegration.iPos_Datamodel.*;
import org.apache.logging.log4j.LogManager;
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.time.LocalDateTime;
import java.time.ZoneId;
import java.time.format.DateTimeFormatter;
import java.util.Date;
import java.util.MissingResourceException;
@Component
public class Odometry {
private final Logger LOG = LoggerFactory.getLogger(getClass());
private static org.apache.logging.log4j.Logger LOG = LogManager.getLogger();
private final String agentId = "";
private static double accuracy;
private static double timeOfLastCalibration;
private static double timeLastSensorEvent;
private static double [] a = new double[]{0, 0, 0};
private static double [] a_ori = new double[]{0, 0, 0};
private static double [] aOffset;
private static double [] v = new double[]{0, 0, 0};
private static double [] p = new double[]{0, 0, 0};
private static double [] delta = new double[]{0, 0, 0};
private static double [] vLast = new double[]{0, 0, 0};
private static double [] pLast = new double[]{0, 0, 0};
private static double NS2S = 1/1000.0;
private static boolean calibrated = false;
static SimpleDateFormat sdf2 = new SimpleDateFormat("yyyy-MM-dd hh:mm:ss.SSS");
//public Odometry(String agentId) {
// this.agentId = agentId;
//}
private static Date convertToDateViaInstant(LocalDateTime dateToConvert) {
return java.util.Date.from(dateToConvert.atZone(ZoneId.systemDefault()).toInstant());
}
private static LocalDateTime convertToLocalDateTimeViaInstant(Date dateToConvert){
return dateToConvert.toInstant().atZone(ZoneId.systemDefault()).toLocalDateTime();
}
public static void calibrate (PositionEvent event) throws ParseException {
v = new double[] {0,0,0};
vLast = new double[] {0,0,0};
aOffset = new double[] {a_ori[0],a_ori[1],a_ori[2]};
accuracy = 0.03;
if (event.getPlacing().getPosition().getPoint() instanceof Point3D) {
pLast[0] = ((Point3D) event.getPlacing().getPosition().getPoint()).getX();
pLast[1] = ((Point3D) event.getPlacing().getPosition().getPoint()).getY();
pLast[2] = ((Point3D) event.getPlacing().getPosition().getPoint()).getZ();
p = pLast;
}
LOG.info("Odometry: timestamp" + event.getTimeStamp());
LocalDateTime eventTime = LocalDateTime.parse(event.getTimeStamp(), DateTimeFormatter.ISO_OFFSET_DATE_TIME);
Date date = convertToDateViaInstant(eventTime);
timeOfLastCalibration = date.getTime();
calibrated = true;
}
public static PositionEvent update(IMU imuRawdataEvent) throws ParseException {
IPos_DatamodelFactory datamodelFactory = IPos_DatamodelFactory.eINSTANCE;
Timestamp timestamp = new Timestamp(System.currentTimeMillis());
Date date = sdf2.parse(imuRawdataEvent.getTimeStamp());
if (calibrated) {
double dt = (date.getTime() - timeLastSensorEvent) * NS2S;
a_ori[0] = imuRawdataEvent.getAcceleration().getX();
a_ori[1] = imuRawdataEvent.getAcceleration().getY();
a_ori[2] = imuRawdataEvent.getAcceleration().getZ();
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) {
v[index] = a[index] * dt + vLast[index];
delta[index] = (v[index] + vLast[index]) / 2 * dt;
p[index] = pLast[index] + delta[index];
pLast[index] = p[index];
vLast[index] = v[index];
}
accuracy = (date.getTime() - timeOfLastCalibration + 500) * 0.0001;
timeLastSensorEvent = date.getTime();
Quaternion calOrientation = datamodelFactory.createQuaternion();
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]);
calOrientation.setX(0);
calOrientation.setY(0);
calOrientation.setZ(0);
calOrientation.setW(0);
calPosition.setPoint(calPoint3D);
calPosition.setAccuracy(calGaussian);
calPosition.setReferenceSystem(DataServices.getReferenceSystemByIdOrNull("ROOT"));
calPlacing.setPosition(calPosition);
calPlacing.setOrientation(calOrientation);
//calLObject.setId(imuRawdataEvent.getSensorId());
LocalDateTime timestamp_LDT=convertToLocalDateTimeViaInstant(date);
calPositionEvent.setLObjectId(imuRawdataEvent.getSensorId());
calPositionEvent.setTimeStamp(timestamp_LDT.toString() + "+00:00");
calPositionEvent.setPlacing(calPlacing);
return calPositionEvent;
}
else {
timeLastSensorEvent = date.getTime();
a_ori[0] = imuRawdataEvent.getAcceleration().getX();
a_ori[1] = imuRawdataEvent.getAcceleration().getY();
a_ori[2] = imuRawdataEvent.getAcceleration().getZ();
throw new MissingResourceException("Odometry not calibrated!", "Odometry", "calibration");
}
}
//TODO
// @JmsListener(destination = "/positions123", containerFactory = "myFactory")
// public void receiveMessage(IposPosition pos) {
......
......@@ -28,7 +28,7 @@ public class SensorDataFusion {
Agent agent = DataServices.getAgentForLocalizableObject(positionEvent.getLObjectId());
List<PositionEvent> eventsForAgent = getEventStorageForAgentOrCreate(agent.getId());
updateRecentEventStorage(eventsForAgent, positionEvent);
return hasHighestAccuracyAmongStoredEvents(positionEvent, eventsForAgent);
return hasBestAccuracyAmongStoredEvents(positionEvent, eventsForAgent);
} catch (IllegalArgumentException e) {
LOG.warn("Sensor-data-fusion could not be applied to position-event as no LocalizableObject with " +
......@@ -47,12 +47,13 @@ public class SensorDataFusion {
* @param eventsForAgent
* @return
*/
private static boolean hasHighestAccuracyAmongStoredEvents(PositionEvent positionEvent, List<PositionEvent> eventsForAgent) {
private static boolean hasBestAccuracyAmongStoredEvents(PositionEvent positionEvent, List<PositionEvent> eventsForAgent) {
for (PositionEvent storedEvent : eventsForAgent){
float storedAccuracy = ((Gaussian) storedEvent.getPlacing().getPosition().getAccuracy()).getConfidenceInterval();
float eventAccuracy = ((Gaussian) positionEvent.getPlacing().getPosition().getAccuracy()).getConfidenceInterval();;
boolean eventsOriginatedFromDifferentSensors = !storedEvent.getLObjectId().equals(positionEvent.getLObjectId()); // important to make sure that the current IMU-position is not hidden behind older ones
logging_sdf(positionEvent, storedEvent);
if (storedAccuracy < eventAccuracy){ // it is important not to use <= here, as at least the current position event itself is contained in the list
if (storedAccuracy < eventAccuracy & eventsOriginatedFromDifferentSensors){ // it is important not to use <= here, as at least the current position event itself is contained in the list
logging_sdf(positionEvent);
return false;
}
......@@ -63,7 +64,7 @@ public class SensorDataFusion {
private static void logging_sdf(PositionEvent positionEvent, PositionEvent storedEvent) {
LocalDateTime eventTime = LocalDateTime.parse(positionEvent.getTimeStamp(), DateTimeFormatter.ISO_OFFSET_DATE_TIME);
LocalDateTime storedTime = LocalDateTime.parse(storedEvent.getTimeStamp(), DateTimeFormatter.ISO_OFFSET_DATE_TIME);
LOG.info("SDF: distance: " + Duration.between(storedTime, eventTime).toMillis());
LOG.info("SDF-DEBUG: distance: " + Duration.between(storedTime, eventTime).toMillis());
}
private static void logging_sdf(PositionEvent positionEvent) {
......@@ -95,7 +96,7 @@ public class SensorDataFusion {
}
LocalDateTime eventTime = LocalDateTime.parse(currEvent.getTimeStamp(), DateTimeFormatter.ISO_OFFSET_DATE_TIME);
long eventAge = Duration.between(eventTime, now).toMillis();
LOG.info("SDF: eventAge: " + eventAge + " ; timestamp: " + currEvent.getTimeStamp() + " ; now: " + now);
LOG.info("SDF-DEBUG: eventAge: " + eventAge + " ; timestamp: " + currEvent.getTimeStamp() + " ; now: " + now);
if ( eventAge > PositionMonitoring.SDF_TIME_INTERVAL){
eventIt.remove();
removeSubsequentElements = true;
......
......@@ -21,6 +21,7 @@ public class MainApp {
public static final String TESTRAWDATA_FILE = "./testdata_raw.txt";
public static final String TESTDATA_INDFRO = "./testdata_raw_indfro.txt";
public static final String TESTDATA_QUERY = "./testdata_raw_query.txt";
public static final String TESTDATA_EMPTY = "./testdata_raw_empty.txt";
public static void main(String[] args) throws ParseException {
......@@ -28,7 +29,7 @@ public class MainApp {
// SimpleSceneIntegration.init();
SimpleSceneIntegration.init(INDUSTRIEROBOTIK_FILE);
// SimpleSceneIntegration.init(SENSORDATENFUSION_FILE);
GenericSensorValueProcessor.processTestData(TESTDATA_INDFRO);
SimpleSceneIntegration.handleMessageFile(TESTDATA_QUERY);
// GenericSensorValueProcessor.processTestData(TESTDATA_EMPTY);
// SimpleSceneIntegration.handleMessageFile(TESTDATA_EMPTY);
}
}
......@@ -6,6 +6,7 @@ import ipos.models.SimpleScene;
import ipos.project.DataModellntegration.iPos_Datamodel.*;
import org.eclipse.emf.ecore.EObject;
import java.time.LocalDateTime;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
......@@ -98,7 +99,10 @@ public class GenericSensorTransformer {
public static NFC nfc_GSensor2Internal(GenericSensor.NFCRawDataEvent proto_nfcRawdataEvent) {
NFC internal_nfcRawdataEvent = modelFactory.createNFC();
internal_nfcRawdataEvent.setTimeStamp(proto_nfcRawdataEvent.getTimestamp());
// internal_nfcRawdataEvent.setTimeStamp(proto_nfcRawdataEvent.getTimestamp());
// fake nfc-timestamp (queube could not provide millisecond-accuracy as it relies on time server)
internal_nfcRawdataEvent.setTimeStamp(LocalDateTime.now() + "+00:00"); // suffix necessary to be parsable by DateTimeFormatter.ISO_OFFSET_DATE_TIME
internal_nfcRawdataEvent.setTagId(proto_nfcRawdataEvent.getTagId());
internal_nfcRawdataEvent.setScannerId(proto_nfcRawdataEvent.getScannerId());
internal_nfcRawdataEvent.setType(proto_nfcRawdataEvent.getType());
......@@ -115,6 +119,23 @@ public class GenericSensorTransformer {
}
return outputMap;
}
public static IMU imu_GSensor2Internal(GenericSensor.IMURawDataEvent proto_imuRawDataEvent) {
IMU internal_imuRawDataEvent = modelFactory.createIMU();
internal_imuRawDataEvent.setTimeStamp(proto_imuRawDataEvent.getTimestamp());
internal_imuRawDataEvent.setSensorId(proto_imuRawDataEvent.getSensorId());
Acceleration internal_acceleration = modelFactory.createAcceleration();
internal_acceleration.setX(proto_imuRawDataEvent.getX());
internal_acceleration.setY(proto_imuRawDataEvent.getY());
internal_acceleration.setZ(proto_imuRawDataEvent.getZ());
internal_imuRawDataEvent.setAcceleration(internal_acceleration);
Quaternion orientation = createOrientation(proto_imuRawDataEvent.getOrientation().getX(),
proto_imuRawDataEvent.getOrientation().getY(),
proto_imuRawDataEvent.getOrientation().getZ(),
proto_imuRawDataEvent.getOrientation().getW());
internal_imuRawDataEvent.setAngularrate(orientation);
return internal_imuRawDataEvent;
}
}
......@@ -7,6 +7,7 @@ import ipos.models.SimpleScene;
import ipos.models.SimpleScene.IposPosition;
import ipos.models.GenericSensor.SensorEventWrapper;
import ipos.project.DataModellntegration.SimpleSceneIntegration.service.impl.ExternalPubServiceImpl;
import ipos.project.DataModellntegration.iPos_Datamodel.IMU;
import ipos.project.DataModellntegration.iPos_Datamodel.NFC;
import ipos.project.DataModellntegration.iPos_Datamodel.PositionEvent;
import ipos.project.SensorValueIntegration.Service.GenericSensorTransformer;
......@@ -54,12 +55,20 @@ public class MqttPositionHandler implements Handler {
processPositionEvents(iposSensorEventWrapper.getSensorPositionEventList());
processNFCRawdataEvents(iposSensorEventWrapper.getNfcRawDataEventList());
processIMURawdataEvents(iposSensorEventWrapper.getImuRawDataEventList());
// this.jmsTemplate.convertAndSend("/request123", monReqInternal); // submit request to the internal broker
}
private static void processIMURawdataEvents(List<GenericSensor.IMURawDataEvent> imuRawDataEventList) {
for (GenericSensor.IMURawDataEvent proto_imuRawDataEvent : imuRawDataEventList){
IMU internal_imuRawDataEvent = GenericSensorTransformer.imu_GSensor2Internal(proto_imuRawDataEvent);
PositionMonitoring.receiveMessage(internal_imuRawDataEvent);
}
}
private static void processNFCRawdataEvents(List<GenericSensor.NFCRawDataEvent> nfcRawDataEventList) {
for (GenericSensor.NFCRawDataEvent proto_nfcRawdataEvent : nfcRawDataEventList){
// transformer fakes nfc-timestamp...
NFC internal_nfcRawDataEvent = GenericSensorTransformer.nfc_GSensor2Internal(proto_nfcRawdataEvent);
PositionMonitoring.receiveMessage(internal_nfcRawDataEvent);
}
......
......@@ -16,6 +16,7 @@ public class Administration {
public Administration(){}
public static void initialize(){
LOG.info("Application initialized");
DataServices.clearWorldModel();
}
......
......@@ -3,6 +3,7 @@ package ipos.project.UseCaseController;
import ipos.project.DataModellntegration.SimpleSceneIntegration.SimpleSceneIntegration;
import ipos.project.DataModellntegration.iPos_Datamodel.*;
import ipos.project.Functionality.DataServices;
import ipos.project.Functionality.Odometry;
import ipos.project.Functionality.PositionCalculation;
import ipos.project.Functionality.SensorDataFusion;
import ipos.project.Functionality.eventfilter.FilteringResult;
......@@ -11,9 +12,9 @@ import ipos.project.Functionality.eventfilter.readConfig;
import org.apache.logging.log4j.LogManager;
import org.springframework.jms.annotation.JmsListener;
import org.springframework.stereotype.Component;
import reactor.util.function.Tuple4;
import java.text.ParseException;
import java.time.LocalDateTime;
import java.util.*;
import java.util.stream.Collectors;
......@@ -47,10 +48,47 @@ public class PositionMonitoring {
PositionEvent positionEvent = PositionCalculation.calcPositionFromNfcOrNull(nfcRawDataEvent);
if (positionEvent != null) {
updateLocalizableObject(positionEvent);
try {
Odometry.calibrate(positionEvent);
} catch (ParseException e) {
LOG.warn("Odometry-component could not be calibrated");
e.printStackTrace();
}
receiveMessage(positionEvent);
}
}
static int imuCount = 0;
public static void receiveMessage(IMU imuRawdataEvent){
PositionEvent positionEvent = null; // if not calibrated: exception
try {
positionEvent = Odometry.update(imuRawdataEvent);
logOdo(positionEvent);
} catch (ParseException e) {
LOG.info("Odometry: ParseException: " + e.getMessage());
} catch (MissingResourceException e){
LOG.info("Odometry: " + e.getMessage());
}
if (positionEvent != null) {
updateLocalizableObject(positionEvent);
if ( imuCount < 5) {
imuCount += 1;
}else {
receiveMessage(positionEvent);
imuCount = 0;
}
}
}
private static void logOdo(PositionEvent positionEvent) {
float odoAccuracy = ((Gaussian) positionEvent.getPlacing().getPosition().getAccuracy()).getConfidenceInterval();
LOG.info("ODO: Odometry calculated position with accuracy: " + odoAccuracy);
}
private static eventFilter getEventFilterByIdOrNull(String monitoringTaskId){
for (eventFilter eFilter : eventFilters){
if (eFilter.getMonitoringTaskId().equals(monitoringTaskId)){
......@@ -62,7 +100,9 @@ public class PositionMonitoring {
private static void updateLocalizableObject(PositionEvent positionEvent) {
LocalizableObject lObject = DataServices.getLObjectByIdOrNull(positionEvent.getLObjectId());
if (lObject == null){
LOG.info("Warning: lObject not found. id contained in positionEvent: " + positionEvent.getLObjectId());
}
// TODO: caution: all changes to the data model should be performed using DataServices.
// Directly changing this object just works for the current simple case that we only have the object-representation of the world model
// in general it has to be made sure that both, all links from this object and to this object keep intact
......@@ -151,13 +191,11 @@ public class PositionMonitoring {
logging_sdfindfro(posEvent);
boolean posEventMeetsSdfAccuracyCondition = SensorDataFusion.isMostAccuratePositionAvailable(posEvent);
List<Tuple4<PositionEvent, String, String, String>> entryNotifications = new LinkedList<>();
List<FilteringResult> filteringResults = new LinkedList<>();
Map<String, List<ZoneDescriptor>> eFilterZoneDescriptors = new HashMap<>();
for (eventFilter eFilter : eventFilters){
try {
if (eFilterMayHandlePosEvent(posEvent, posEventMeetsSdfAccuracyCondition, eFilter)){
filteringResults.add(applyEfilterToPosEvent(posEvent, entryNotifications, eFilter));
filteringResults.add(applyEfilterToPosEvent(posEvent, eFilter));
}
} catch (ParseException e) {
processFilteringResults(posEvent, filteringResults);
......@@ -347,12 +385,13 @@ public class PositionMonitoring {
LocalizableObject lObject = PositionMonitoring.getLObjectByIdOrNull(posEvent.getLObjectId());
if (lObject == null){
LOG.info("INDFRO: Warning, unknown sensorId was found. Can not log!");
return;
}
String sensorType = lObject.getSensorType();
String timestamp = posEvent.getTimeStamp();
String sensorId = posEvent.getLObjectId();
String agentId = lObject.getAgent().getId();
LOG.info("SDF: Received: sensorType: " + sensorType + " timestamp: " + timestamp + " sensorId: " + sensorId + " agentId: " + agentId);
LOG.info("SDF: Received: sensorType: " + sensorType + " timestamp: " + timestamp + " now: " + LocalDateTime.now() + " sensorId: " + sensorId + " agentId: " + agentId);
LOG.info("INDFRO:");
LOG.info("INDFRO: Received: sensorType: " + sensorType + "; position: " + logPosition(posEvent) + "; timestamp: " + timestamp + "; sensorId: " + sensorId + "; agentId: " + agentId);
}
......@@ -365,7 +404,9 @@ public class PositionMonitoring {
/**
* An event-filter may hot handle a position event if the filter should respect the results of
* sensor data fusion (e.g., wrt accuracy-criterium) and the result is negative for this event,
* i.e., the event did not pass sensor data fusion and was rejected.
* i.e., the event did not pass sensor data fusion and was rejected. If sensor-data-fusion is not
* relevant, the eFilter should handle the event regardless of whether it was sorted out by sdf or
* not. If it is relevant, the eFilter should block the event if it was sorted out by sdf.
* @param posEvent
* @param posEventMeetsSdfAccuracyCondition
* @param eFilter
......@@ -376,7 +417,7 @@ public class PositionMonitoring {
return !sdfIsRelevant || (sdfIsRelevant && posEventMeetsSdfAccuracyCondition);
}
private static FilteringResult applyEfilterToPosEvent(PositionEvent posEvent, List<Tuple4<PositionEvent, String, String, String>> entryNotifications, eventFilter eFilter) throws ParseException {
private static FilteringResult applyEfilterToPosEvent(PositionEvent posEvent, eventFilter eFilter) throws ParseException {
FilteringResult filteringResult = eFilter.process(posEvent);
boolean posEventMeetsFilterConditions = !filteringResult.hasBlockedEvent();
if(posEventMeetsFilterConditions) {
......
......@@ -12,8 +12,10 @@ mqtt:
setKeepAliveInterval: 10
connectionTimeout: 20
clientId: ipos.project.fw
# hostname: 192.168.0.111
hostname: broker.hivemq.com
# hostname: 192.168.0.111 # ceti-laptop robolab
# hostname: broker.hivemq.com # online
hostname: 192.168.0.143 # Frank-laptop robolab
# hostname: localhost
port: 1883
spring:
......
{"sensorPositionEvent": [{"sensorId": "UWB_1", "position": {"refSystemId": "ROOT", "point": {"x": 3.0,"y": 1.5,"z": 3.0}, "accuracy": 1.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "lastPosUpdate": "2021-10-14T19:32:20+00:00"}]}
{"sensorPositionEvent": [{"sensorId": "UWB_1", "position": {"refSystemId": "ROOT", "point": {"x": 3.0,"y": 4.5,"z": 3.0}, "accuracy": 1.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "lastPosUpdate": "2021-10-14T19:32:21+00:00"}]}
{"sensorPositionEvent": [{"sensorId": "UWB_1", "position": {"refSystemId": "ROOT", "point": {"x": -3.0,"y": 4.5,"z": 3.0}, "accuracy": 1.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "lastPosUpdate": "2021-10-14T19:32:22+00:00"}]}
{"sensorPositionEvent": [{"sensorId": "UWB_1", "position": {"refSystemId": "ROOT", "point": {"x": 3.0,"y": 1.5,"z": 3.0}, "accuracy": 1.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "lastPosUpdate": "2021-10-14T19:32:23+00:00"}]}
{"sensorPositionEvent": [{"sensorId": "UWB_1", "position": {"refSystemId": "ROOT", "point": {"x": 3.0,"y": 1.5,"z": 3.0}, "accuracy": 10.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "lastPosUpdate": "2021-10-14T19:32:20+00:00"}]}
{"sensorPositionEvent": [{"sensorId": "UWB_1", "position": {"refSystemId": "ROOT", "point": {"x": 3.0,"y": 4.5,"z": 3.0}, "accuracy": 10.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "lastPosUpdate": "2021-10-14T19:32:21+00:00"}]}
{"sensorPositionEvent": [{"sensorId": "UWB_1", "position": {"refSystemId": "ROOT", "point": {"x": -3.0,"y": 4.5,"z": 3.0}, "accuracy": 10.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "lastPosUpdate": "2021-10-14T19:32:22+00:00"}]}
{"sensorPositionEvent": [{"sensorId": "UWB_1", "position": {"refSystemId": "ROOT", "point": {"x": 3.0,"y": 1.5,"z": 3.0}, "accuracy": 10.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "lastPosUpdate": "2021-10-14T19:32:23+00:00"}]}
{"nfcRawDataEvent" : [ {"timestamp" : "2021-10-14T19:33:24+00:00", "tagId": "83221710", "type": "RFID", "scannerId": "98:CD:AC:26:2D:18"} ] }
{"sensorPositionEvent": [{"sensorId": "UWB_1", "position": {"refSystemId": "ROOT", "point": {"x": 3.0,"y": 1.5,"z": 3.0}, "accuracy": 1.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "lastPosUpdate": "2021-10-14T19:34:25+00:00"}]}
\ No newline at end of file
{"sensorPositionEvent": [{"sensorId": "UWB_1", "position": {"refSystemId": "ROOT", "point": {"x": 3.0,"y": 1.5,"z": 3.0}, "accuracy": 10.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "lastPosUpdate": "2021-10-14T19:34:25+00:00"}]}
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment