implemented reportMachine (not tested yet).

robocup-logistics · Apr 18, 2024 · 17f7061 · 17f7061
1 parent 7bb2bf2
commit 17f7061
Show file tree

Hide file tree

Showing 2 changed files with 46 additions and 2 deletions.
diff --git a/app/src/main/java/com/rcll/java/RefboxTeamHandler.java b/app/src/main/java/com/rcll/java/RefboxTeamHandler.java
@@ -7,8 +7,7 @@
 import org.eclipse.paho.client.mqttv3.*;
 
 import java.nio.charset.StandardCharsets;
-import java.util.HashMap;
-import java.util.Map;
+import java.util.*;
 import java.util.function.Consumer;
 
 @CommonsLog
@@ -31,6 +30,8 @@ public class RefboxTeamHandler implements MqttCallback {
     private final String beaconRobot2Topic;
     private final String beaconRobot3Topic;
 
+    private final String reportMachineTopic;
+
     public RefboxTeamHandler(IMqttClient mqttClient, RefboxClient refboxClient, String teamName) {
         this.mqttClient = mqttClient;
         this.refboxClient = refboxClient;
@@ -47,6 +48,7 @@ public RefboxTeamHandler(IMqttClient mqttClient, RefboxClient refboxClient, Stri
         beaconRobot1Topic = teamName + "/beacon/R1";
         beaconRobot2Topic = teamName + "/beacon/R2";
         beaconRobot3Topic = teamName + "/beacon/R3";
+        reportMachineTopic = teamName + "/report";
 
         this.callbacks.put(prepareBsInputTopic, this::prepareBsInput);
         this.callbacks.put(prepareBsOutputTopic, this::prepareBsOutput);
@@ -58,6 +60,7 @@ public RefboxTeamHandler(IMqttClient mqttClient, RefboxClient refboxClient, Stri
         this.callbacks.put(beaconRobot1Topic, (s) -> this.sendRobotBeaconSignal(1, s));
         this.callbacks.put(beaconRobot2Topic, (s) -> this.sendRobotBeaconSignal(2, s));
         this.callbacks.put(beaconRobot3Topic, (s) -> this.sendRobotBeaconSignal(3, s));
+        this.callbacks.put(reportMachineTopic, this::reportMachine);
     }
 
     private void prepareRs1(String s) {
@@ -125,6 +128,34 @@ private void prepareBsInput(String s) {
         }
     }
 
+    private void reportMachine(String dataStr) {
+        try {
+            ReportMachineData data = objectMapper.readValue(dataStr, ReportMachineData.class);
+            this.refboxClient.sendReportMachine(new MachineName(data.machine), this.coordinateToZone(data.x, data.y),  this.discreticiseYaw(data.yaw));
+        } catch (Exception ex) {
+            log.warn("Error on sending Report signal for Machine: [" + dataStr + "]! ", ex);
+        }
+    }
+
+    private int discreticiseYaw(float yaw) {
+        if (yaw > 10) {
+            log.warn("discreticiseYaw excpets value in RADIANS!");
+        }
+        Integer[] arr = {0, 45, 90, 135, 180, 225, 270, 315, 360};
+        Vector<Integer> steps = new Vector<>(Arrays.asList(arr));
+        int angle = (int)Math.toDegrees(yaw);
+        Collections.sort(steps, Comparator.comparing(p -> Math.abs(p - angle)));
+        return steps.get(0) % 360;
+    }
+
+    private ZoneName coordinateToZone(float x, float y) {
+        Integer zoneX = (int)Math.floor(x) + 1;
+        Integer zoneY = (int)Math.floor(y) + 1;
+
+
+        return new ZoneName("M_Z" + zoneX + zoneY);
+    }
+
     public void start() throws MqttException {
         this.mqttClient.subscribe(new String[]{
                 prepareBsInputTopic, prepareBsOutputTopic, prepareCs1Topic,

diff --git a/app/src/main/java/com/rcll/java/ReportMachineData.java b/app/src/main/java/com/rcll/java/ReportMachineData.java
@@ -0,0 +1,13 @@
+package com.rcll.java;
+
+import lombok.Getter;
+import lombok.Setter;
+
+@Getter
+@Setter
+public class ReportMachineData {
+    String machine;
+    float x;
+    float y;
+    float yaw;
+}