diff --git a/init_Industrierobotik.txt b/init_Industrierobotik.txt
index 84a0f0e71754bc39bd2b05a46d048eb120d38eea..110c7fc6fd8f9f10590ea11be69b6ccb257c0ba5 100644
--- a/init_Industrierobotik.txt
+++ b/init_Industrierobotik.txt
@@ -1,5 +1,5 @@
-{"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
diff --git a/init_Sensordatenfusion.txt b/init_Sensordatenfusion.txt
index 12a97a98e641d6a4759b957df7892d8819a1e193..a79b0ffe8ba43eb9b40a560fd4524c0d9c8d7a08 100644
--- a/init_Sensordatenfusion.txt
+++ b/init_Sensordatenfusion.txt
@@ -1,5 +1,5 @@
{"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
diff --git a/src/main/java/ipos/project/CustomLoggingFilter.java b/src/main/java/ipos/project/CustomLoggingFilter.java
index 826a31fc8bb5cefc9967011f5a85637f760a9070..9d302951e0d73fe32406594b425780f7cbfdab31 100644
--- a/src/main/java/ipos/project/CustomLoggingFilter.java
+++ b/src/main/java/ipos/project/CustomLoggingFilter.java
@@ -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;
}
}
diff --git a/src/main/java/ipos/project/DataModellntegration/SimpleSceneIntegration/SimpleSceneIntegration.java b/src/main/java/ipos/project/DataModellntegration/SimpleSceneIntegration/SimpleSceneIntegration.java
index 2246adc6dcd1566a6175f9de6d14b23dda7789b4..9c566996f02e0f5a96344a568e15e81675727bc4 100644
--- a/src/main/java/ipos/project/DataModellntegration/SimpleSceneIntegration/SimpleSceneIntegration.java
+++ b/src/main/java/ipos/project/DataModellntegration/SimpleSceneIntegration/SimpleSceneIntegration.java
@@ -1,14 +1,13 @@
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;
+ }
+
}
diff --git a/src/main/java/ipos/project/DataModellntegration/SimpleSceneIntegration/service/SimpleSceneTransformer.java b/src/main/java/ipos/project/DataModellntegration/SimpleSceneIntegration/service/SimpleSceneTransformer.java
index 9454e3304bda9a133ccc0f0a04f1e7a2dba31f23..14caf9ee6c7401cb5b944b13951b0b70015daf8c 100644
--- a/src/main/java/ipos/project/DataModellntegration/SimpleSceneIntegration/service/SimpleSceneTransformer.java
+++ b/src/main/java/ipos/project/DataModellntegration/SimpleSceneIntegration/service/SimpleSceneTransformer.java
@@ -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;
}
diff --git a/src/main/java/ipos/project/Functionality/DataServices.java b/src/main/java/ipos/project/Functionality/DataServices.java
index 748a6a5e7618db238b2d1ffe6f25f9cc66e17b99..955387ee99957f8a2f79a38dd1ca212241d32310 100644
--- a/src/main/java/ipos/project/Functionality/DataServices.java
+++ b/src/main/java/ipos/project/Functionality/DataServices.java
@@ -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)){
diff --git a/src/main/java/ipos/project/Functionality/Odometry.java b/src/main/java/ipos/project/Functionality/Odometry.java
index 876404b2bddfcf033b08d69afcbc08286e7f077f..eecd1863e7caa6d28af866f85eb4aa2a99ca2caa 100644
--- a/src/main/java/ipos/project/Functionality/Odometry.java
+++ b/src/main/java/ipos/project/Functionality/Odometry.java
@@ -1,15 +1,129 @@
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) {
diff --git a/src/main/java/ipos/project/Functionality/SensorDataFusion.java b/src/main/java/ipos/project/Functionality/SensorDataFusion.java
index 0971f019a985e4906ee8d3524739b811bddef083..20a73640f2978fc741efa9e019df447eb4885b48 100644
--- a/src/main/java/ipos/project/Functionality/SensorDataFusion.java
+++ b/src/main/java/ipos/project/Functionality/SensorDataFusion.java
@@ -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;
diff --git a/src/main/java/ipos/project/MainApp.java b/src/main/java/ipos/project/MainApp.java
index 0dbfb419ab088d98bd80c44e2e72d7267f3d8473..c66b05ec70cb513a96e37747a9bfda833c7ed78a 100644
--- a/src/main/java/ipos/project/MainApp.java
+++ b/src/main/java/ipos/project/MainApp.java
@@ -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);
}
}
diff --git a/src/main/java/ipos/project/SensorValueIntegration/Service/GenericSensorTransformer.java b/src/main/java/ipos/project/SensorValueIntegration/Service/GenericSensorTransformer.java
index 46b0ec393c9252796e5cdd70dc92b98afc2d7eb3..e6481e3adec0da613d7d54524856a47a9d722a8a 100644
--- a/src/main/java/ipos/project/SensorValueIntegration/Service/GenericSensorTransformer.java
+++ b/src/main/java/ipos/project/SensorValueIntegration/Service/GenericSensorTransformer.java
@@ -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,8 +99,11 @@ 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.setTagId(proto_nfcRawdataEvent.getTagId());
+ // 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());
internal_nfcRawdataEvent.setTagData(convertToStringToObjectMap(proto_nfcRawdataEvent.getTagDataMap()));
@@ -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;
+ }
}
diff --git a/src/main/java/ipos/project/SensorValueIntegration/api/MqttPositionHandler.java b/src/main/java/ipos/project/SensorValueIntegration/api/MqttPositionHandler.java
index d44a68abd70ae185738b54a1c9d948e69fdb45a9..b12ae6eab46ba11f88db16634066057b11374c37 100644
--- a/src/main/java/ipos/project/SensorValueIntegration/api/MqttPositionHandler.java
+++ b/src/main/java/ipos/project/SensorValueIntegration/api/MqttPositionHandler.java
@@ -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);
}
diff --git a/src/main/java/ipos/project/UseCaseController/Administration.java b/src/main/java/ipos/project/UseCaseController/Administration.java
index 827ea2056a882d53d9a67b621ac5b44c8c10675d..6d62afd04054b84e7ee9171e1ff18d02d576b03e 100644
--- a/src/main/java/ipos/project/UseCaseController/Administration.java
+++ b/src/main/java/ipos/project/UseCaseController/Administration.java
@@ -16,6 +16,7 @@ public class Administration {
public Administration(){}
public static void initialize(){
+ LOG.info("Application initialized");
DataServices.clearWorldModel();
}
diff --git a/src/main/java/ipos/project/UseCaseController/PositionMonitoring.java b/src/main/java/ipos/project/UseCaseController/PositionMonitoring.java
index 3ca1693fc0340e1f8e6f2e20e8e9dce846ae96da..07b19bf5d7d2b5078e4ee621038c6c08e41df614 100644
--- a/src/main/java/ipos/project/UseCaseController/PositionMonitoring.java
+++ b/src/main/java/ipos/project/UseCaseController/PositionMonitoring.java
@@ -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) {
diff --git a/src/main/resources/application.yml b/src/main/resources/application.yml
index 517faebecf521f3821ba39de797a502cf99020f2..7382dbc7eeab8154aee788e31c68ae9c76702cbc 100644
--- a/src/main/resources/application.yml
+++ b/src/main/resources/application.yml
@@ -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:
diff --git a/testdata_raw_indfro.txt b/testdata_raw_indfro.txt
index 73712114be453211a0c860e9f76d332bd4f9853f..daeb3592d38d7ebb30777617d105fb55723f6825 100644
--- a/testdata_raw_indfro.txt
+++ b/testdata_raw_indfro.txt
@@ -1,6 +1,6 @@
-{"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