From 7d20d803149abcbea6a7910522383bd84ddaaae8 Mon Sep 17 00:00:00 2001
From: Oleksandr Husak <oleksandr.husak@mailbox.tu-dresden.de>
Date: Mon, 25 Apr 2022 13:53:07 +0200
Subject: [PATCH] way points from new topic

---
 README.md                               | 125 +++++++++++++++++-------
 src/app/components/map/map.component.ts |  79 ++++++++++++++-
 src/app/model/base-model.ts             |  25 ++++-
 3 files changed, 185 insertions(+), 44 deletions(-)

diff --git a/README.md b/README.md
index c797cf3..fb19aa7 100644
--- a/README.md
+++ b/README.md
@@ -30,49 +30,98 @@ Connection settings
 - 2. Send messages to a `hivemq` public broker
     - topic `ipos/client/position`
         - new positions
-        - ```json
-                {
-                "objects": [
+            - ```json
                     {
-                    "id": "Employee1",
-                    "sensorId": "UWB_1",
-                    "type": "HUMAN",
-                    "sensorType": "UWB",
-                    "position": {
-                        "refSystemId": "ROOT",
-                        "point": {
-                        "x": 28,
-                        "y": -12,
-                        "z": 20
-                        },
-                        "accuracy": 1
-                    },
-                    "orientation": {
-                        "x": 0,
-                        "y": 0,
-                        "z": -0.7071,
-                        "w": 0.7071
-                    },
-                    "extractedAttributes": {
-                        "batteryChargeLevel": 70,
-                        "loadedItems": [23, 1, 25, 17],
-                        "errors": [2, 1, 6],
-                        "theta": -0.9
-                    },
-                    "lastPosUpdate": "2021-09-14T09:41:20+00:00",
-                    "zoneDescriptors": [
+                    "objects": [
                         {
-                        "zoneId": "door_zone",
-                        "notificationType": "EntryNotification"
+                        "id": "Employee1",
+                        "sensorId": "UWB_1",
+                        "type": "HUMAN",
+                        "sensorType": "UWB",
+                        "position": {
+                            "refSystemId": "ROOT",
+                            "point": {
+                            "x": 28,
+                            "y": -12,
+                            "z": 20
+                            },
+                            "accuracy": 1
+                        },
+                        "orientation": {
+                            "x": 0,
+                            "y": 0,
+                            "z": -0.7071,
+                            "w": 0.7071
+                        },
+                        "extractedAttributes": {
+                            "batteryChargeLevel": 70,
+                            "loadedItems": [23, 1, 25, 17],
+                            "errors": [2, 1, 6],
+                            "theta": -0.9
+                        },
+                        "lastPosUpdate": "2021-09-14T09:41:20+00:00",
+                        "zoneDescriptors": [
+                            {
+                            "zoneId": "door_zone",
+                            "notificationType": "EntryNotification"
+                            }
+                        ]
                         }
-                    ]
+                    ],
+                    "type": "EntryNotification"
                     }
-                ],
-                "type": "EntryNotification"
-                }
-          ```
+            ```
             - More objects can be added to the `"object": []`
-            - There are special colours for objects with id `"Employee1"` - `"Employee4"`. Config: [MarkerColorMap](src/environments/environment.ts)
+            - There are special colours for objects. Config: [MarkerColorList](src/environments/environment.ts)
+    - topic `ipos/client/way`
+        - new way points
+            - ```json
+                [
+                    {
+                        "agentId":"Employee1",
+                        "agentType":"HUMAN",
+                        "publisher":"main",
+                        "position":{
+                            "refSystemId":"ROOT",
+                            "point":{
+                                "x":58,
+                                "y":42,
+                                "z":20
+                            }
+                        },
+                        "time":"2022-04-14T09:41:20+00:00"
+                    },
+                    {
+                        "agentId":"Employee1",
+                        "agentType":"HUMAN",
+                        "publisher":"main",
+                        "position":{
+                            "refSystemId":"ROOT",
+                            "point":{
+                                "x":18,
+                                "y":12,
+                                "z":50
+                            }
+                        },
+                        "time":"2022-04-14T09:41:20+00:00"
+                    },
+                    {
+                        "agentId":"Employee2",
+                        "agentType":"ROBOT",
+                        "publisher":"Employee2",
+                        "position":{
+                            "refSystemId":"ROOT",
+                            "point":{
+                                "x":-18,
+                                "y":-52,
+                                "z":33
+                            }
+                        },
+                        "time":"2022-04-14T10:41:20+00:00"
+                    }
+                ]
+
+            ```
     - topic `ipos/client/root`
         - ```json
             {
diff --git a/src/app/components/map/map.component.ts b/src/app/components/map/map.component.ts
index ff0f43a..200622b 100644
--- a/src/app/components/map/map.component.ts
+++ b/src/app/components/map/map.component.ts
@@ -6,7 +6,16 @@ import 'leaflet-arrowheads';
 import { IMqttMessage, MqttService } from 'ngx-mqtt';
 
 import { environment, getMarkerConfig} from 'src/environments/environment';
-import {Position, PositionUpdate, RelativePos, WGS84, validateTypePosition, validateTypeAgent} from 'src/app/model/base-model';
+import {
+  Position, 
+  PositionUpdate, 
+  RelativePos, 
+  WGS84, 
+  validateTypePosition, 
+  validateTypeAgent, 
+  WayPoint,
+  isWayPoint
+} from 'src/app/model/base-model';
 
 
 @Component({
@@ -18,17 +27,20 @@ export class MapComponent implements OnInit {
 
   private subsPosition: Subscription;
   private subsRoot: Subscription;
+  private subsWayPoint: Subscription;
 
   private map!: L.Map;
   private root!: Position;
   
   agentsInfo: { [key: string]: {} } = {};
   markOverlays: { [key: string]: L.LayerGroup<any> } = {}; 
+  wayPointsOverlays: { [key: string]: L.LayerGroup<any> } = {}; 
   posOverlays: { [key: string]: L.LayerGroup<any> } = {};
 
   constructor(private _mqttService: MqttService) {
     this.root = {"refSystemId": "ROOT", "point": {"latitude": 51.02545, "longitude": 13.72295}}
 
+    // --- Root
     this.subsRoot = this._mqttService.observe('ipos/client/root').subscribe((message: IMqttMessage) => {
       try {
         let root = <Position>JSON.parse(message.payload.toString())
@@ -41,6 +53,7 @@ export class MapComponent implements OnInit {
       }
     });
 
+    // --- Agent position
     this.subsPosition = this._mqttService.observe('ipos/client/position').subscribe((message: IMqttMessage) => {
       try {
         let upd: PositionUpdate = <PositionUpdate>JSON.parse(message.payload.toString())
@@ -53,6 +66,24 @@ export class MapComponent implements OnInit {
           console.log(e)
       }
     });
+
+
+    // --- Agent way points
+    this.subsWayPoint = this._mqttService.observe('ipos/client/way').subscribe((message: IMqttMessage) => {
+      try {
+        let points: WayPoint[] = <WayPoint[]>JSON.parse(message.payload.toString())
+        
+        console.log(points)
+
+        points.forEach(wayPoint => {
+          if (isWayPoint(wayPoint)) {
+            this.registerWayPoint(wayPoint)
+          }
+        });
+      } catch(e) {
+          console.log(e)
+      }
+    });
   }
 
   ngOnInit(): void { } 
@@ -86,8 +117,6 @@ export class MapComponent implements OnInit {
       //   }
       // });
 
-
-
     } else {
       this.markOverlays[key] = L.layerGroup().addTo(<L.Map>this.map)
     }
@@ -111,6 +140,50 @@ export class MapComponent implements OnInit {
     this.addMarker(key, pos, props, desc.extractedAttributes?.theta)
   }
 
+  registerWayPoint(wayPoint: WayPoint) {
+    // empty position
+    if (wayPoint.position==undefined) {
+      console.log("Error: empty position.")
+      return
+    }
+
+    // empty position
+    if (Object.values(wayPoint.position).every((e)=>{e==undefined})) {
+      console.log("Error: empty point.")
+      return;
+    }
+
+    let key = wayPoint.agentId
+
+    // --- Marker config
+    let serialNum = Object.keys(this.markOverlays).indexOf(key)
+    var markerConf = getMarkerConfig(serialNum)
+
+    // --- Layer on the map
+    if (!(key in this.wayPointsOverlays)) {
+      this.wayPointsOverlays[key] = L.layerGroup().addTo(<L.Map>this.map)
+    }
+
+    let point = wayPoint.position.point
+
+    if ('x' in point) {
+      var globPos = this.ref2root(point)
+    } else {
+      var globPos = L.latLng([point.latitude, point.longitude]);
+    }
+
+    // --- Marker
+
+    let markerDesc = `<p> \
+      <h4>Way point</h4>
+      <strong>Agent</strong>: ${wayPoint.agentId} <br>\
+      <strong>Time</strong>: ${wayPoint.time} <br>
+    </p>`
+
+    let marker = L.circleMarker(globPos, {radius: 5, color: markerConf.color}).bindPopup(markerDesc).openPopup();
+    marker.addTo(this.wayPointsOverlays[key])
+  }
+
   addMarker(key: string, pos: Position, popup: string, theta: number) {
 
     // empty position
diff --git a/src/app/model/base-model.ts b/src/app/model/base-model.ts
index 30864c0..19520ba 100644
--- a/src/app/model/base-model.ts
+++ b/src/app/model/base-model.ts
@@ -7,7 +7,7 @@ export interface PositionUpdate {
 export interface Agent {
     id: string
     sensorId: string
-    type: ObjectType
+    type: AgentType
     sensorType: SensorType
     position: Position
     orientation?: Quaternion
@@ -53,7 +53,7 @@ export enum NotificationType {
     "Unknown"
 }
 
-enum ObjectType {
+enum AgentType {
     "HUMAN",
     "BOX",
     "ROBOT"
@@ -108,4 +108,23 @@ export function isAgent(obj: any): obj is Agent {
         && "id" in obj
         && "position" in obj
         && isPosition(obj.position)
-} 
\ No newline at end of file
+} 
+
+
+// --- Way points
+
+export interface WayPoint {
+    "agentId": string
+    "agentType"?: AgentType
+    "publisher"?: string
+    "position": Position
+    "time": string
+}
+
+
+export function isWayPoint(obj: any): obj is WayPoint {
+    return obj
+        && "agentId" in obj
+        && "time" in obj
+        && isPosition(obj.position)
+} 
-- 
GitLab