fixed bugs related to placing

case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/handlers/ControlEndHandler.java

@@ -15,15 +15,7 @@ public class ControlEndHandler extends TransitionHandler {
     final static Function<List<Map<String, Object>>, List<Map<String, Object>>> END_HANDLING_FUNCTION = maps -> {
 
         System.out.println("[RESULT_HANDLING_FUNCTION] Executing.");
-
-
-        RobotConnector robotConnector = RobotConnector.getInstance();
-        try {
-            robotConnector.setup(Configurations.MQTT_BROKER_ADDRESS);
-            robotConnector.listenToScenes();
-        } catch (MqttException e) {
-            e.printStackTrace();
-        }
+        RobotConnector rc = RobotConnector.getInstance();
         String arm = null;
 
         for (Map.Entry<String, Object> entry : maps.get(0).entrySet()) {
@@ -31,30 +23,28 @@ public class ControlEndHandler extends TransitionHandler {
                 maps.get(0).replace(TokenConstants.LOCKED,"true");
             }
             if (entry.getKey().equals(TokenConstants.COLOR)) {
-                if(robotConnector.leftRobotObjectTypes.contains(entry.getValue())){
-                    arm = robotConnector.ROBOT_LEFT;
+                if(rc.leftRobotObjectTypes.contains(entry.getValue())){
+                    arm = rc.ROBOT_LEFT;
                 } else {
-                    arm = robotConnector.ROBOT_RIGHT;
+                    arm = rc.ROBOT_RIGHT;
                 }
             }
         }
 
         if(arm != null){
             try {
-                robotConnector.isEvacuating = true;
-                robotConnector.currentRobot = arm;
+                rc.isEvacuating = true;
 
-                while(!robotConnector.leftRobotState.equals(de.tudresden.inf.st.ceti.Object.State.STATE_IDLE)
-                        && !robotConnector.rightRobotState.equals(de.tudresden.inf.st.ceti.Object.State.STATE_IDLE)) {}
+                while(!rc.leftRobotState.equals(de.tudresden.inf.st.ceti.Object.State.STATE_IDLE)
+                        && !rc.rightRobotState.equals(de.tudresden.inf.st.ceti.Object.State.STATE_IDLE)) {}
 
-                robotConnector.sendEvacuate(arm, true);
+                rc.sendEvacuate(arm, true);
 
                 System.out.println("[RESULT_HANDLING_FUNCTION] Waiting after evacuate command.");
-                while (robotConnector.isEvacuating) {
+                while (rc.isEvacuating) {
                     Thread.onSpinWait();
                 }
 
-                robotConnector.currentRobot = null;
             } catch (MqttException e) {
                 e.printStackTrace();
             }

case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/handlers/PickHandler.java

@@ -5,6 +5,7 @@ import de.tudresden.inf.st.ceti.Pick;
 import de.tudresden.inf.st.ceti.PickOrBuilder;
 import de.tudresden.inf.st.pnml.engine.execution.TransitionHandler;
 import de.tudresden.inf.st.sorting.constants.TokenConstants;
+import de.tudresden.inf.st.sorting.mqtt.Configurations;
 import de.tudresden.inf.st.sorting.mqtt.MqttUtils;
 import de.tudresden.inf.st.sorting.mqtt.RobotConnector;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
@@ -21,12 +22,13 @@ public class PickHandler extends TransitionHandler {
         System.out.println("[PICK_HANDLING_FUNCTION] Executing.");
         List<Map<String, Object>> res = new ArrayList<>();
         RobotConnector rc = RobotConnector.getInstance();
+        rc.setup(Configurations.MQTT_BROKER_ADDRESS);
 
         for(Map<String, Object> m : maps){
 
             // never went through that transition
             if(!m.get(TokenConstants.TRACE).toString().contains("pickToSafety") &&
-                    m.get(TokenConstants.TRACE).toString().contains("safety")){
+                    !m.get(TokenConstants.TRACE).toString().contains("safety")){
                 System.out.println("[PICK_HANDLING_FUNCTION] Picking.");
 
                 String arm;
@@ -36,6 +38,9 @@ public class PickHandler extends TransitionHandler {
                     arm = rc.ROBOT_RIGHT;
                 }
 
+
+                System.out.println("[PICK_HANDLING_FUNCTION] Sending pick command: " +
+                        arm + " / " + m.get(TokenConstants.NAME).toString());
                 rc.sendPick(arm, m.get(TokenConstants.NAME).toString());
 
             }

case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/handlers/PlaceHandler.java

@@ -17,9 +17,13 @@ public class PlaceHandler extends TransitionHandler {
 
         List<Map<String, Object>> res = new ArrayList<>();
         RobotConnector rc = RobotConnector.getInstance();
+        rc.setup(Configurations.MQTT_BROKER_ADDRESS);
 
         for (Map<String, Object> m : maps) {
 
+            System.out.println("Token: \n");
+            printToken(m);
+
             // never went through that transition
             if (m.get(TokenConstants.PICK_SUCCESS).toString().equals("success") &&
                     !m.get(TokenConstants.TRACE).toString().contains("placeToSafety")) {
@@ -29,21 +33,37 @@ public class PlaceHandler extends TransitionHandler {
                 String arm;
                 if (rc.leftRobotObjectTypes.contains(m.get(TokenConstants.COLOR))) {
                     arm = rc.ROBOT_LEFT;
+                    System.out.println("[PLACE_HANDLING_FUNCTION] 1.");
                 } else {
                     arm = rc.ROBOT_RIGHT;
+                    System.out.println("[PLACE_HANDLING_FUNCTION] 2.");
                 }
 
+                System.out.println("[PLACE_HANDLING_FUNCTION] 3. ");
+
                 String bin;
                 if(m.get(TokenConstants.COLOR).toString().equals(Configurations.SFX_BLUE)){
                     bin = Configurations.BIN_BLUE;
+                    System.out.println("[PLACE_HANDLING_FUNCTION] 4.");
                 } else if(m.get(TokenConstants.COLOR).toString().equals(Configurations.SFX_GREEN)){
                     bin = Configurations.BIN_GREEN;
+                    System.out.println("[PLACE_HANDLING_FUNCTION] 5.");
                 } else {
                     bin = Configurations.BIN_RED;
+                    System.out.println("[PLACE_HANDLING_FUNCTION] 6.");
                 }
 
+                System.out.println("[PLACE_HANDLING_FUNCTION] 7.");
+
+                System.out.println("B: left: " + rc.leftRobotState);
+                System.out.println("B: right: " + rc.leftRobotState);
+
                 while(!rc.leftRobotState.equals(de.tudresden.inf.st.ceti.Object.State.STATE_IDLE)
-                        && !rc.rightRobotState.equals(de.tudresden.inf.st.ceti.Object.State.STATE_IDLE)) {}
+                        && !rc.rightRobotState.equals(de.tudresden.inf.st.ceti.Object.State.STATE_IDLE)) {
+                    System.out.println("left: " + rc.leftRobotState);
+                    System.out.println("right: " + rc.leftRobotState);
+                }
+                System.out.println("[PLACE_HANDLING_FUNCTION] Sending place command: " + arm + " / " + bin);
                 rc.sendPlace(arm, bin);
 
             }
@@ -62,6 +82,12 @@ public class PlaceHandler extends TransitionHandler {
         return res;
     };
 
+    private static void printToken(Map<String, Object> t){
+        for (Map.Entry<String, Object> entry : t.entrySet()) {
+            System.out.println(entry.getKey() + ":" + entry.getValue());
+        }
+    }
+
     public PlaceHandler(int priority) {
         super(priority, PLACE_HANDLING_FUNCTION);
     }

case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/mqtt/Configurations.java

@@ -8,6 +8,9 @@ public interface Configurations {
     String LEFT_SCENE_UPDATE_TOPIC = "/ceti_cell_1/scene/delta-update";
     String RIGHT_SCENE_UPDATE_TOPIC = "/ceti_cell_2/scene/delta-update";
 
+    String LEFT_SCENE_TOPIC = "/ceti_cell_1/scene/update";
+    String RIGHT_SCENE_TOPIC = "/ceti_cell_2/scene/update";
+
     String WEB_SELECTION_TOPIC = "selection";
 
     String MQTT_BROKER_ADDRESS = "tcp://localhost:1883";
@@ -16,7 +19,7 @@ public interface Configurations {
     String BIN_RED = "binRed";
     String BIN_GREEN = "binGreen";
 
-    String SFX_BLUE = "Blue";
-    String SFX_RED = "Red";
-    String SFX_GREEN = "Green";
+    String SFX_BLUE = "blue";
+    String SFX_RED = "red";
+    String SFX_GREEN = "green";
 }

case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/mqtt/RobotConnector.java

@@ -20,19 +20,29 @@ public class RobotConnector extends Connector {
     private final String clientId = UUID.randomUUID().toString();
     public static IMqttClient client = null;
     public volatile boolean isEvacuating = false;
-    public String currentRobot = null;
     private boolean isListening = false;
     public Object.State leftRobotState = null;
+    public Object.State leftPredRobotState = null;
     public Object.State rightRobotState = null;
+    public Object.State rightPredRobotState = null;
+    private boolean setup = false;
 
-    public void setup(String mqttHost) throws MqttException {
+    public void setup(String mqttHost) {
+        try {
+            if(!setup) {
+                setup = true;
                 client = new MqttClient(mqttHost, clientId, new MemoryPersistence());
                 MqttUtils.setupMqttOptions(client);
             }
+        } catch (MqttException e) {
+            e.printStackTrace();
+        }
+    }
 
     public Object.State getRobotState(String arm, Scene scene){
         for(Object o : scene.getObjectsList()){
             if(o.getId().equals(arm)){
+                System.out.println("Robot State: " + o.getState());
                 return o.getState();
             }
         }
@@ -51,15 +61,44 @@ public class RobotConnector extends Connector {
 
                 @Override
                 public void messageArrived(String topic, MqttMessage message) throws Exception {
-                    if (topic.equals(Configurations.LEFT_SCENE_UPDATE_TOPIC)){
+
+                    System.out.println("[MQTT] Received message on topic: " + topic);
+
+                    if (topic.equals(Configurations.LEFT_SCENE_UPDATE_TOPIC) ||
+                            topic.equals(Configurations.LEFT_SCENE_TOPIC)){
                         Scene s = Scene.parseFrom(message.getPayload());
-                        checkIfEvacuated(s);
+                        System.out.println("[MQTT] Updating left robot state.");
                         leftRobotState = getRobotState(ROBOT_LEFT, s);
                     }
-                    if (topic.equals(Configurations.RIGHT_SCENE_UPDATE_TOPIC)) {
+                    if (topic.equals(Configurations.RIGHT_SCENE_UPDATE_TOPIC) ||
+                            topic.equals(Configurations.RIGHT_SCENE_TOPIC)) {
                         Scene s = Scene.parseFrom(message.getPayload());
-                        checkIfEvacuated(s);
-                        leftRobotState = getRobotState(ROBOT_RIGHT, s);
+                        System.out.println("[MQTT] Updating right robot state.");
+                      /*  Object.State newState = getRobotState(ROBOT_RIGHT, s);
+
+                        if(leftRobotState == null){
+                            leftRobotState = newState;
+                        }
+
+                        if(!newState.toString().equals(leftRobotState.toString())){
+                            leftPredRobotState = leftRobotState;
+                            leftRobotState = newState;
+                            System.out.println("[MQTT] Detected robot state change: "
+                                    + leftPredRobotState + " >> " + leftRobotState);
+                        }
+
+                        if(leftRobotState.toString().equals("STATE_IDLE") &&
+                                leftPredRobotState.toString().equals("STATE_PICKING")){
+                            System.out.println("[MQTT] Submitting pick success signal.");
+                            client.publish("PickSuccess", MqttUtils.buildMsg("true".getBytes()));
+                        }*/
+
+
+                        for(Object o : s.getObjectsList()){
+                            if(o.getId().equals("smallGreen")){
+                                System.out.println("State: " + o.getState());
+                            }
+                        }
                     }
                 }
 
@@ -69,21 +108,9 @@ public class RobotConnector extends Connector {
             });
 
             client.subscribe(Configurations.LEFT_SCENE_UPDATE_TOPIC, 0);
+            client.subscribe(Configurations.LEFT_SCENE_TOPIC, 0);
             client.subscribe(Configurations.RIGHT_SCENE_UPDATE_TOPIC, 0);
-        }
-    }
-
-    private void checkIfEvacuated(Scene scene) {
-
-        for (Object o : scene.getObjectsList()) {
-            System.out.println(o.getType() + " -- " + o.getId() + " -- " + currentRobot);
-            if (o.getType().equals(Object.Type.ARM) && o.getId().equals(currentRobot)) {
-                System.out.println("[RobotConnector] Arm State: " + o.getState().name());
-                if (o.getState().equals(Object.State.STATE_IDLE)) {
-                    isEvacuating = false;
-                    return;
-                }
-            }
+            client.subscribe(Configurations.RIGHT_SCENE_TOPIC, 0);
         }
     }
 
@@ -95,11 +122,13 @@ public class RobotConnector extends Connector {
                 .build();
 
         Command com = Command.newBuilder().setPick(p).build();
+        System.out.println("com: " + com);
         MqttMessage comMsg = MqttUtils.buildMsg(com.toByteArray());
+        System.out.println("comMsg: " + comMsg);
 
-        System.out.println("[RobotConnector] Constructed pick command: " + object + " via robot: " + currentRobot);
+        System.out.println("[RobotConnector] Constructed pick command: " + object + " via robot: " + arm);
 
-        if (currentRobot.equals(ROBOT_LEFT)) {
+        if (arm.equals(ROBOT_LEFT)) {
             System.out.println("[RobotConnector] Sending pick command to left robot.");
             try {
                 client.publish(Configurations.LEFT_COMMAND_TOPIC, comMsg);
@@ -108,7 +137,7 @@ public class RobotConnector extends Connector {
             }
         }
 
-        if (currentRobot.equals(ROBOT_RIGHT)) {
+        if (arm.equals(ROBOT_RIGHT)) {
             System.out.println("[RobotConnector] Sending pick command to right robot.");
             try {
                 client.publish(Configurations.RIGHT_COMMAND_TOPIC, comMsg);
@@ -120,18 +149,18 @@ public class RobotConnector extends Connector {
 
     public void sendPlace(String arm, String object) {
 
-        Place p = Place.newBuilder()
-                .setIdPlace(object)
+        Drop d = Drop.newBuilder()
+                .setIdBin(object)
                 .setIdRobot(arm)
                 .build();
 
-        Command com = Command.newBuilder().setPlace(p).build();
+        Command com = Command.newBuilder().setDrop(d).build();
         MqttMessage comMsg = MqttUtils.buildMsg(com.toByteArray());
 
-        System.out.println("[RobotConnector] Constructed place command: " + object + " via robot: " + currentRobot);
+        System.out.println("[RobotConnector] Constructed drop command: " + object + " via robot: " + arm);
 
-        if (currentRobot.equals(ROBOT_LEFT)) {
-            System.out.println("[RobotConnector] Sending place command to left robot.");
+        if (arm.equals(ROBOT_LEFT)) {
+            System.out.println("[RobotConnector] Sending drop command to left robot.");
             try {
                 client.publish(Configurations.LEFT_COMMAND_TOPIC, comMsg);
             } catch (MqttException e) {
@@ -139,8 +168,8 @@ public class RobotConnector extends Connector {
             }
         }
 
-        if (currentRobot.equals(ROBOT_RIGHT)) {
-            System.out.println("[RobotConnector] Sending place command to right robot.");
+        if (arm.equals(ROBOT_RIGHT)) {
+            System.out.println("[RobotConnector] Sending drop command to right robot.");
             try {
                 client.publish(Configurations.RIGHT_COMMAND_TOPIC, comMsg);
             } catch (MqttException e) {

case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/nodes/ExecutorNode.java

@@ -3,6 +3,9 @@ package de.tudresden.inf.st.sorting.nodes;
 import de.tudresden.inf.st.pnml.engine.ros.DiNeRosNode;
 import de.tudresden.inf.st.pnml.jastadd.model.*;
 import de.tudresden.inf.st.sorting.constants.TokenConstants;
+import de.tudresden.inf.st.sorting.mqtt.Configurations;
+import de.tudresden.inf.st.sorting.mqtt.RobotConnector;
+import org.eclipse.paho.client.mqttv3.MqttException;
 
 import java.util.ArrayList;
 import java.util.List;
@@ -14,6 +17,14 @@ public class ExecutorNode extends DiNeRosNode {
 
     public ExecutorNode(String nodeName, PetriNet petriNet, String rcHost) {
         super(nodeName, petriNet, rcHost, "mqtt");
+
+        RobotConnector rc = RobotConnector.getInstance();
+        try {
+            rc.setup(Configurations.MQTT_BROKER_ADDRESS);
+            rc.listenToScenes();
+        } catch (MqttException e) {
+            e.printStackTrace();
+        }
     }
 
     public boolean hasIntervalPassed() {

case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/nodes/SynchronizerNode.java

@@ -2,10 +2,21 @@ package de.tudresden.inf.st.sorting.nodes;
 
 import de.tudresden.inf.st.pnml.engine.ros.DiNeRosDefaultNode;
 import de.tudresden.inf.st.pnml.jastadd.model.PetriNet;
+import de.tudresden.inf.st.sorting.mqtt.Configurations;
+import de.tudresden.inf.st.sorting.mqtt.RobotConnector;
+import org.eclipse.paho.client.mqttv3.MqttException;
 
 public class SynchronizerNode extends DiNeRosDefaultNode {
 
     public SynchronizerNode(String nodeName, PetriNet petriNet, String rcHost) {
         super(nodeName, petriNet, rcHost, "mqtt");
+
+        RobotConnector rc = RobotConnector.getInstance();
+        try {
+            rc.setup(Configurations.MQTT_BROKER_ADDRESS);
+            rc.listenToScenes();
+        } catch (MqttException e) {
+            e.printStackTrace();
+        }
     }
 }

case-study-sorting-pkg/src/main/resources/nets/SelectorNode.pnml

@@ -18,7 +18,7 @@
                         <subnet>selectorGreen</subnet>
                         <balloonMarking>
                             <tokens>
-                                <token>{"color" : "green", "name" : "green1", "pickSuccess" : "false", "placeSuccess" : "false",
+                                <token>{"color" : "green", "name" : "smallGreen", "pickSuccess" : "false", "placeSuccess" : "false",
                                         "humanDetected" : "false", "sensorData" : "", "trace" : "", "locked" : "false" } </token>
                             </tokens>
                         </balloonMarking>
@@ -50,7 +50,7 @@
                         <subnet>selectorBlue</subnet>
                         <balloonMarking>
                             <tokens>
-                                <token>{"color" : "blue", "name" : "blue1", "pickSuccess" : "false", "placeSuccess" : "false",
+                                <token>{"color" : "blue", "name" : "smallBlue", "pickSuccess" : "false", "placeSuccess" : "false",
                                     "humanDetected" : "false", "sensorData" : "", "trace" : "", "locked" : "false" } </token>
                             </tokens>
                         </balloonMarking>
@@ -70,7 +70,7 @@
                         <subnet>selectorRed</subnet>
                         <balloonMarking>
                             <tokens>
-                                <token>{"color" : "red", "name" : "red1", "pickSuccess" : "false", "placeSuccess" : "false",
+                                <token>{"color" : "red", "name" : "smallRed", "pickSuccess" : "false", "placeSuccess" : "false",
                                     "humanDetected" : "false", "sensorData" : "", "trace" : "", "locked" : "false" } </token>
                             </tokens>
                         </balloonMarking>