diff --git a/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/handlers/ControlEndHandler.java b/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/handlers/ControlEndHandler.java index e09087019269eb35d18633b004e57e9b45e957e7..66e2bfb3f8a9071718ba414a0318598edfd3986b 100644 --- a/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/handlers/ControlEndHandler.java +++ b/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(); } diff --git a/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/handlers/PickHandler.java b/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/handlers/PickHandler.java index 44b9f6232513a269373146d302e34ed350b2f243..6fb1d7bf773a77c4582c72874de371a0e7d4039c 100644 --- a/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/handlers/PickHandler.java +++ b/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()); } diff --git a/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/handlers/PlaceHandler.java b/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/handlers/PlaceHandler.java index 2946091a8a9ec5a3613f69ff83197c153fbdc6ae..f5d82e6b5541d52d2562a2eeba3c713b6d5aa3b9 100644 --- a/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/handlers/PlaceHandler.java +++ b/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); } diff --git a/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/mqtt/Configurations.java b/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/mqtt/Configurations.java index 510f9df2ae59a4134e0734858b2dad9c936d608f..6e5fb16a0d85fca3ba95354e11bb3476521b562b 100644 --- a/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/mqtt/Configurations.java +++ b/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"; } diff --git a/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/mqtt/RobotConnector.java b/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/mqtt/RobotConnector.java index 1c0993312581ec73cea39b9d24be0d196b53d958..692d9f007d712de96d93b90364d92a01b71e6256 100644 --- a/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/mqtt/RobotConnector.java +++ b/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 void setup(String mqttHost) throws MqttException { - client = new MqttClient(mqttHost, clientId, new MemoryPersistence()); - MqttUtils.setupMqttOptions(client); + public Object.State rightPredRobotState = null; + private boolean setup = false; + + 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) { diff --git a/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/nodes/ExecutorNode.java b/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/nodes/ExecutorNode.java index ece0d05821b0c8552cbf032112cbd55ecc85a82e..91c904132b28dc57d22f08b81b6e854210a53cbd 100644 --- a/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/nodes/ExecutorNode.java +++ b/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() { diff --git a/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/nodes/SynchronizerNode.java b/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/nodes/SynchronizerNode.java index 04a547e07dfb3da012fb0fc7c4e78b93e6b09b9e..c48eb982d68c6766d332d24dbadde0abec7704cd 100644 --- a/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/nodes/SynchronizerNode.java +++ b/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(); + } } } diff --git a/case-study-sorting-pkg/src/main/resources/nets/SelectorNode.pnml b/case-study-sorting-pkg/src/main/resources/nets/SelectorNode.pnml index c914a69407a531fba657afc87fba6788533f9cd6..37ca4f3bf793a688f9fd01ab4840532c6ab28fa0 100644 --- a/case-study-sorting-pkg/src/main/resources/nets/SelectorNode.pnml +++ b/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>