diff --git a/case-study-sorting-pkg/build.gradle b/case-study-sorting-pkg/build.gradle index 98433bf1d5d57e224ac4225bb7ef43c777727315..eb8ff0dfdc7d795d46d14695a8df2d09a534518c 100644 --- a/case-study-sorting-pkg/build.gradle +++ b/case-study-sorting-pkg/build.gradle @@ -1,16 +1,23 @@ // General configuration (plugins, settings, dependencies) buildscript { repositories.mavenCentral() +} +plugins { + id 'java' + id 'application' + id 'idea' + id "com.google.protobuf" version "0.8.18" } group 'de.tudresden.inf.st' version '0.1' -apply plugin: 'java' -apply plugin: 'java-library' -apply plugin: 'application' -apply plugin: "idea" +protobuf { + protoc { + artifact = "com.google.protobuf:protoc:4.0.0-rc-2" + } +} java.toolchain.languageVersion.set(JavaLanguageVersion.of(11)) @@ -27,6 +34,15 @@ dependencies { compile fileTree(include: ['pnml-relast-engine-fatjar-0.1.jar'], dir: './libs') testCompile group: 'junit', name: 'junit', version: '4.12' + + implementation group: 'com.google.protobuf', name: 'protobuf-java', version: "4.0.0-rc-2" + implementation group: 'com.google.protobuf', name: 'protobuf-java-util', version: "4.0.0-rc-2" + + // https://mvnrepository.com/artifact/org.eclipse.paho/org.eclipse.paho.client.mqttv3 + implementation group: 'org.eclipse.paho', name: 'org.eclipse.paho.client.mqttv3', version: '1.2.5' + + // https://mvnrepository.com/artifact/commons-io/commons-io + implementation group: 'commons-io', name: 'commons-io', version: '2.11.0' } jar { 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 c9f912c745f5df8fe23a9ce185e0a0a358af2581..e09087019269eb35d18633b004e57e9b45e957e7 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 @@ -2,6 +2,9 @@ package de.tudresden.inf.st.sorting.handlers; import de.tudresden.inf.st.pnml.engine.execution.TransitionHandler; import de.tudresden.inf.st.sorting.constants.TokenConstants; +import de.tudresden.inf.st.sorting.mqtt.RobotConnector; +import de.tudresden.inf.st.sorting.mqtt.Configurations; +import org.eclipse.paho.client.mqttv3.MqttException; import java.util.List; import java.util.Map; @@ -10,16 +13,55 @@ import java.util.function.Function; public class ControlEndHandler extends TransitionHandler { final static Function<List<Map<String, Object>>, List<Map<String, Object>>> END_HANDLING_FUNCTION = maps -> { - System.out.println("Executing RESULT_HANDLING_FUNCTION."); - for(Map<String, Object> m : maps){ - for (Map.Entry<String, Object> entry : m.entrySet()) { - if(entry.getKey().equals(TokenConstants.LOCKED)){ - m.replace(TokenConstants.LOCKED, "true"); + 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(); + } + String arm = null; + + for (Map.Entry<String, Object> entry : maps.get(0).entrySet()) { + if (entry.getKey().equals(TokenConstants.LOCKED)) { + maps.get(0).replace(TokenConstants.LOCKED,"true"); + } + if (entry.getKey().equals(TokenConstants.COLOR)) { + if(robotConnector.leftRobotObjectTypes.contains(entry.getValue())){ + arm = robotConnector.ROBOT_LEFT; + } else { + arm = robotConnector.ROBOT_RIGHT; } } } + if(arm != null){ + try { + robotConnector.isEvacuating = true; + robotConnector.currentRobot = arm; + + 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)) {} + + robotConnector.sendEvacuate(arm, true); + + System.out.println("[RESULT_HANDLING_FUNCTION] Waiting after evacuate command."); + while (robotConnector.isEvacuating) { + Thread.onSpinWait(); + } + + robotConnector.currentRobot = null; + } catch (MqttException e) { + e.printStackTrace(); + } + } else { + System.out.println("[RESULT_HANDLING_FUNCTION] Failed to evacuate: no arm defined for color."); + } + return maps; }; 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 a826857e554413d749b8d1cb838b25f6df5bc907..44b9f6232513a269373146d302e34ed350b2f243 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 @@ -1,7 +1,13 @@ package de.tudresden.inf.st.sorting.handlers; +import de.tudresden.inf.st.ceti.Command; +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.MqttUtils; +import de.tudresden.inf.st.sorting.mqtt.RobotConnector; +import org.eclipse.paho.client.mqttv3.MqttMessage; import java.util.ArrayList; import java.util.List; @@ -11,20 +17,31 @@ import java.util.function.Function; public class PickHandler extends TransitionHandler { final static Function<List<Map<String, Object>>, List<Map<String, Object>>> PICK_HANDLING_FUNCTION = maps -> { - System.out.println("Executing PICK_HANDLING_FUNCTION."); - - try { - Thread.sleep(1000); - System.out.println("Fake picking finished."); - } catch (InterruptedException e) { - e.printStackTrace(); - } + System.out.println("[PICK_HANDLING_FUNCTION] Executing."); List<Map<String, Object>> res = new ArrayList<>(); + RobotConnector rc = RobotConnector.getInstance(); for(Map<String, Object> m : maps){ - if(!m.get(TokenConstants.TRACE).toString().contains("safety")){ - String oldTrace = m.get(TokenConstants.TRACE).toString(); + + // never went through that transition + if(!m.get(TokenConstants.TRACE).toString().contains("pickToSafety") && + m.get(TokenConstants.TRACE).toString().contains("safety")){ + System.out.println("[PICK_HANDLING_FUNCTION] Picking."); + + String arm; + if(rc.leftRobotObjectTypes.contains(m.get(TokenConstants.COLOR))){ + arm = rc.ROBOT_LEFT; + } else { + arm = rc.ROBOT_RIGHT; + } + + rc.sendPick(arm, m.get(TokenConstants.NAME).toString()); + + } + + String oldTrace = m.get(TokenConstants.TRACE).toString(); + if(!oldTrace.contains("safety")){ m.replace(TokenConstants.TRACE, oldTrace + "-pickToSafety"); res.add(m); } 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 9ffb63c34deaa4b52fac3c4c87f0589b9521147f..2946091a8a9ec5a3613f69ff83197c153fbdc6ae 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 @@ -2,6 +2,8 @@ package de.tudresden.inf.st.sorting.handlers; 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.RobotConnector; import java.util.ArrayList; import java.util.List; @@ -11,24 +13,50 @@ import java.util.function.Function; public class PlaceHandler extends TransitionHandler { final static Function<List<Map<String, Object>>, List<Map<String, Object>>> PLACE_HANDLING_FUNCTION = maps -> { - System.out.println("Executing PLACE_HANDLING_FUNCTION."); - - try { - Thread.sleep(1000); - System.out.println("Fake placing finished."); - } catch (InterruptedException e) { - e.printStackTrace(); - } + System.out.println("[PLACE_HANDLING_FUNCTION] Executing."); List<Map<String, Object>> res = new ArrayList<>(); + RobotConnector rc = RobotConnector.getInstance(); + + for (Map<String, Object> m : maps) { + + // never went through that transition + if (m.get(TokenConstants.PICK_SUCCESS).toString().equals("success") && + !m.get(TokenConstants.TRACE).toString().contains("placeToSafety")) { + + System.out.println("[PLACE_HANDLING_FUNCTION] Placing."); + + String arm; + if (rc.leftRobotObjectTypes.contains(m.get(TokenConstants.COLOR))) { + arm = rc.ROBOT_LEFT; + } else { + arm = rc.ROBOT_RIGHT; + } - for(Map<String, Object> m : maps){ - if(!m.get(TokenConstants.TRACE).toString().contains("safety") - && (!m.get(TokenConstants.TRACE).toString().contains("pickToSafety") - || !m.get(TokenConstants.PICK_SUCCESS).toString().equals("false"))){ + String bin; + if(m.get(TokenConstants.COLOR).toString().equals(Configurations.SFX_BLUE)){ + bin = Configurations.BIN_BLUE; + } else if(m.get(TokenConstants.COLOR).toString().equals(Configurations.SFX_GREEN)){ + bin = Configurations.BIN_GREEN; + } else { + bin = Configurations.BIN_RED; + } + + 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.sendPlace(arm, bin); + + } + + + String oldTrace = m.get(TokenConstants.TRACE).toString(); + if (!oldTrace.contains("safety") && (!oldTrace.contains("pickToSafety") + || m.get(TokenConstants.PICK_SUCCESS).toString().equals("success"))) { + m.replace(TokenConstants.TRACE, oldTrace + "-placeToSafety"); res.add(m); return res; } + } return res; diff --git a/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/handlers/SortColorHandler.java b/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/handlers/SortColorHandler.java index 1cee2098a3802654186ff5ab408e93b1b9440635..98d0329b2d7b1947c3e01fe747c4c91cb6628674 100644 --- a/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/handlers/SortColorHandler.java +++ b/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/handlers/SortColorHandler.java @@ -20,6 +20,7 @@ public class SortColorHandler extends TransitionHandler { } } } + return maps; }; diff --git a/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/launcher/NodeLauncher.java b/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/launcher/NodeLauncher.java index 737560a6e3fad7c20d23890e9118ec8ef893626f..255aaf3fa0a3807914f6e067680473590bdf57c3 100644 --- a/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/launcher/NodeLauncher.java +++ b/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/launcher/NodeLauncher.java @@ -4,11 +4,17 @@ import org.ros.node.DefaultNodeMainExecutor; import org.ros.node.NodeConfiguration; import org.ros.node.NodeMainExecutor; +import java.util.ArrayList; +import java.util.HashSet; +import java.util.List; +import java.util.Set; + public abstract class NodeLauncher { protected static final String ROS_HOST = "localhost"; protected static final String ROS_MASTER_URI = "http://localhost:11311"; protected static final String MQTT_HOST = "localhost"; + protected static final String MQTT_REMAP_HOST = "tcp://localhost:1883"; protected static final NodeConfiguration nodeConfiguration = NodeConfiguration.newPublic(ROS_HOST); protected static final NodeMainExecutor nodeMainExecutor = DefaultNodeMainExecutor.newDefault(); diff --git a/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/launcher/SelectorLauncher.java b/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/launcher/SelectorLauncher.java index 983e223d42528c6c94a5b29c8005b79a9bc5fc95..b934f405a153112fdded2c8d18bfcb12dc279c25 100644 --- a/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/launcher/SelectorLauncher.java +++ b/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/launcher/SelectorLauncher.java @@ -3,7 +3,9 @@ package de.tudresden.inf.st.sorting.launcher; import de.tudresden.inf.st.pnml.jastadd.model.PetriNet; import de.tudresden.inf.st.pnml.jastadd.model.PnmlParser; import de.tudresden.inf.st.sorting.handlers.SortColorHandler; +import de.tudresden.inf.st.sorting.mqtt.SignalConnector; import de.tudresden.inf.st.sorting.nodes.SelectorNode; +import org.eclipse.paho.client.mqttv3.MqttException; import java.net.URI; @@ -21,6 +23,14 @@ public class SelectorLauncher extends NodeLauncher{ selectorNode.registerHandler("SortRed", new SortColorHandler(1)); selectorNode.registerHandler("SortBlue", new SortColorHandler(1)); + SignalConnector sm = new SignalConnector(); + try { + sm.setup(MQTT_REMAP_HOST); + sm.remapSelection(); + } catch (MqttException e) { + e.printStackTrace(); + } + new Thread(() -> nodeMainExecutor.execute(selectorNode, nodeConfiguration)) {{start();}}; } } 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 new file mode 100644 index 0000000000000000000000000000000000000000..510f9df2ae59a4134e0734858b2dad9c936d608f --- /dev/null +++ b/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/mqtt/Configurations.java @@ -0,0 +1,22 @@ +package de.tudresden.inf.st.sorting.mqtt; + +public interface Configurations { + + String LEFT_COMMAND_TOPIC = "/ceti_cell_1/command"; + String RIGHT_COMMAND_TOPIC = "/ceti_cell_2/command"; + + String LEFT_SCENE_UPDATE_TOPIC = "/ceti_cell_1/scene/delta-update"; + String RIGHT_SCENE_UPDATE_TOPIC = "/ceti_cell_2/scene/delta-update"; + + String WEB_SELECTION_TOPIC = "selection"; + + String MQTT_BROKER_ADDRESS = "tcp://localhost:1883"; + + String BIN_BLUE = "binBlue"; + String BIN_RED = "binRed"; + String BIN_GREEN = "binGreen"; + + 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/Connector.java b/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/mqtt/Connector.java new file mode 100644 index 0000000000000000000000000000000000000000..5c74265b8fe7a912dee818809e2b9a6cdbc4ce99 --- /dev/null +++ b/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/mqtt/Connector.java @@ -0,0 +1,10 @@ +package de.tudresden.inf.st.sorting.mqtt; + +import java.util.HashSet; +import java.util.Set; + +public abstract class Connector { + + public final Set<String> leftRobotObjectTypes = new HashSet<>(Set.of("Red", "Blue")); + public final Set<String> rightRobotObjectTypes = new HashSet<>(Set.of("Green")); +} diff --git a/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/mqtt/MqttUtils.java b/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/mqtt/MqttUtils.java new file mode 100644 index 0000000000000000000000000000000000000000..5f963afeb3226ddb10a12ca17c4ef832f55c6287 --- /dev/null +++ b/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/mqtt/MqttUtils.java @@ -0,0 +1,27 @@ +package de.tudresden.inf.st.sorting.mqtt; + +import de.tudresden.inf.st.ceti.Scene; +import org.eclipse.paho.client.mqttv3.*; + +public class MqttUtils { + + public static void sendScene(String topic, Scene scene, IMqttClient client) throws MqttException { + System.out.println("[MQTT] Sending initial scene."); + MqttMessage msg = buildMsg(scene.toByteArray()); + client.publish(topic, msg); + } + + public static MqttMessage buildMsg(byte[] bytes) { + MqttMessage msg = new MqttMessage(bytes); + msg.setQos(0); + return msg; + } + + public static void setupMqttOptions(IMqttClient client) throws MqttException { + MqttConnectOptions options = new MqttConnectOptions(); + options.setAutomaticReconnect(true); + options.setCleanSession(true); + options.setConnectionTimeout(10); + client.connect(options); + } +} 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 new file mode 100644 index 0000000000000000000000000000000000000000..1c0993312581ec73cea39b9d24be0d196b53d958 --- /dev/null +++ b/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/mqtt/RobotConnector.java @@ -0,0 +1,184 @@ +package de.tudresden.inf.st.sorting.mqtt; + +import de.tudresden.inf.st.ceti.*; +import de.tudresden.inf.st.ceti.Object; +import org.eclipse.paho.client.mqttv3.*; +import org.eclipse.paho.client.mqttv3.persist.MemoryPersistence; + +import java.util.UUID; + +public class RobotConnector extends Connector { + + private static final RobotConnector INSTANCE = new RobotConnector(); + + private RobotConnector() {} + + public static RobotConnector getInstance() { return INSTANCE; } + + public String ROBOT_LEFT = "arm2"; + public String ROBOT_RIGHT = "arm1"; + 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 rightRobotState = null; + + public void setup(String mqttHost) throws MqttException { + client = new MqttClient(mqttHost, clientId, new MemoryPersistence()); + MqttUtils.setupMqttOptions(client); + } + + public Object.State getRobotState(String arm, Scene scene){ + for(Object o : scene.getObjectsList()){ + if(o.getId().equals(arm)){ + return o.getState(); + } + } + return null; + } + + public void listenToScenes() throws MqttException { + + if(!isListening) { + isListening = true; + client.setCallback(new MqttCallback() { + @Override + public void connectionLost(Throwable cause) { + System.out.println("[MQTT] Connection lost: " + cause.getMessage()); + } + + @Override + public void messageArrived(String topic, MqttMessage message) throws Exception { + if (topic.equals(Configurations.LEFT_SCENE_UPDATE_TOPIC)){ + Scene s = Scene.parseFrom(message.getPayload()); + checkIfEvacuated(s); + leftRobotState = getRobotState(ROBOT_LEFT, s); + } + if (topic.equals(Configurations.RIGHT_SCENE_UPDATE_TOPIC)) { + Scene s = Scene.parseFrom(message.getPayload()); + checkIfEvacuated(s); + leftRobotState = getRobotState(ROBOT_RIGHT, s); + } + } + + @Override + public void deliveryComplete(IMqttDeliveryToken token) { + } + }); + + client.subscribe(Configurations.LEFT_SCENE_UPDATE_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; + } + } + } + } + + public void sendPick(String arm, String object) { + + Pick p = Pick.newBuilder() + .setIdPick(object) + .setIdRobot(arm) + .build(); + + Command com = Command.newBuilder().setPick(p).build(); + MqttMessage comMsg = MqttUtils.buildMsg(com.toByteArray()); + + System.out.println("[RobotConnector] Constructed pick command: " + object + " via robot: " + currentRobot); + + if (currentRobot.equals(ROBOT_LEFT)) { + System.out.println("[RobotConnector] Sending pick command to left robot."); + try { + client.publish(Configurations.LEFT_COMMAND_TOPIC, comMsg); + } catch (MqttException e) { + e.printStackTrace(); + } + } + + if (currentRobot.equals(ROBOT_RIGHT)) { + System.out.println("[RobotConnector] Sending pick command to right robot."); + try { + client.publish(Configurations.RIGHT_COMMAND_TOPIC, comMsg); + } catch (MqttException e) { + e.printStackTrace(); + } + } + } + + public void sendPlace(String arm, String object) { + + Place p = Place.newBuilder() + .setIdPlace(object) + .setIdRobot(arm) + .build(); + + Command com = Command.newBuilder().setPlace(p).build(); + MqttMessage comMsg = MqttUtils.buildMsg(com.toByteArray()); + + System.out.println("[RobotConnector] Constructed place command: " + object + " via robot: " + currentRobot); + + if (currentRobot.equals(ROBOT_LEFT)) { + System.out.println("[RobotConnector] Sending place command to left robot."); + try { + client.publish(Configurations.LEFT_COMMAND_TOPIC, comMsg); + } catch (MqttException e) { + e.printStackTrace(); + } + } + + if (currentRobot.equals(ROBOT_RIGHT)) { + System.out.println("[RobotConnector] Sending place command to right robot."); + try { + client.publish(Configurations.RIGHT_COMMAND_TOPIC, comMsg); + } catch (MqttException e) { + e.printStackTrace(); + } + } + } + + public void sendEvacuate(String robot, boolean useMoveToPose) throws MqttException { + + Evacuate e = Evacuate.newBuilder() + .setIdRobot(robot) + .setIdCollaborationZone("none") + .build(); + + MoveToPose mtp = MoveToPose.newBuilder() + .setIdRobot(robot) + .setPosition(Object.Position.newBuilder().setX(-0.32).setY(-0.4).setZ(1.49).build()) + .setOrientation(Object.Orientation.newBuilder().setX(1.0).setY(0.0).setZ(0.0).setW(0.0).build()) + .build(); + + Command command; + + if(useMoveToPose){ + command = Command.newBuilder().setMoveToPose(mtp).build(); + } else { + command = Command.newBuilder().setEvacuate(e).build(); + } + + MqttMessage msg = MqttUtils.buildMsg(command.toByteArray()); + + + if (robot.equals(ROBOT_LEFT)) { + client.publish(Configurations.LEFT_COMMAND_TOPIC, msg); + } + + if (robot.equals(ROBOT_RIGHT)) { + client.publish(Configurations.RIGHT_COMMAND_TOPIC, msg); + } + } +} diff --git a/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/mqtt/SceneReader.java b/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/mqtt/SceneReader.java new file mode 100644 index 0000000000000000000000000000000000000000..102f1b747e1268683354b608add6c69064b7767d --- /dev/null +++ b/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/mqtt/SceneReader.java @@ -0,0 +1,24 @@ +package de.tudresden.inf.st.sorting.mqtt; + +import com.google.protobuf.util.JsonFormat; +import de.tudresden.inf.st.ceti.Scene; +import org.apache.commons.io.IOUtils; + +import java.io.IOException; +import java.io.InputStream; +import java.nio.charset.StandardCharsets; + +public class SceneReader { + + public static Scene readFromJson(String filePath) throws IOException { + + ClassLoader classLoader = SceneReader.class.getClassLoader(); + InputStream inputStream = classLoader.getResourceAsStream(filePath); + assert inputStream != null; + String sceneJson = IOUtils.toString(inputStream, StandardCharsets.UTF_8); + + Scene.Builder sceneBuilder = Scene.newBuilder(); + JsonFormat.parser().ignoringUnknownFields().merge(sceneJson, sceneBuilder); + return sceneBuilder.build(); + } +} diff --git a/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/mqtt/SignalConnector.java b/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/mqtt/SignalConnector.java new file mode 100644 index 0000000000000000000000000000000000000000..292570bf8dee2ed37070cf713f4a84f308e00049 --- /dev/null +++ b/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/mqtt/SignalConnector.java @@ -0,0 +1,57 @@ +package de.tudresden.inf.st.sorting.mqtt; + +import de.tudresden.inf.st.ceti.Selection; +import org.eclipse.paho.client.mqttv3.*; +import org.eclipse.paho.client.mqttv3.persist.MemoryPersistence; + +import java.util.UUID; + +public class SignalConnector extends Connector { + + private final String clientId = UUID.randomUUID().toString(); + public static IMqttClient client = null; + + public void setup(String mqttHost) throws MqttException { + + client = new MqttClient(mqttHost, clientId, new MemoryPersistence()); + MqttUtils.setupMqttOptions(client); + } + + public void remapSelection() throws MqttException { + + client.setCallback(new MqttCallback() { + @Override + public void connectionLost(Throwable cause) { + System.out.println("[MQTT] Connection lost: " + cause.getMessage()); + } + + @Override + public void messageArrived(String topic, MqttMessage message) throws Exception { + + System.out.println("[MQTT] Received webinterface selection."); + + if(topic.equals(Configurations.WEB_SELECTION_TOPIC)){ + Selection selection = Selection.parseFrom(message.getPayload()); + + String[] split = selection.getId().split("-"); + + MqttMessage msg; + if(split[1].equals("True")){ + msg = MqttUtils.buildMsg("true".getBytes()); + System.out.println("[MQTT] Setting " + split[0] + " to true."); + } else { + msg = MqttUtils.buildMsg("false".getBytes()); + System.out.println("[MQTT] Setting " + split[0] + " to false."); + } + client.publish(split[0], msg); + } + } + + @Override + public void deliveryComplete(IMqttDeliveryToken token) {} + }); + + client.subscribe(Configurations.WEB_SELECTION_TOPIC, 0); + + } +} diff --git a/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/nodes/SelectorNode.java b/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/nodes/SelectorNode.java index 87f9e075afd04393e3b48aee18c1c828e128adbd..37e181951813d40b524b98beaca5ddea6241bb77 100644 --- a/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/nodes/SelectorNode.java +++ b/case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/nodes/SelectorNode.java @@ -1,12 +1,45 @@ package de.tudresden.inf.st.sorting.nodes; +import de.tudresden.inf.st.ceti.Scene; import de.tudresden.inf.st.pnml.engine.ros.DiNeRosDefaultNode; import de.tudresden.inf.st.pnml.jastadd.model.*; +import de.tudresden.inf.st.sorting.mqtt.MqttUtils; +import de.tudresden.inf.st.sorting.mqtt.SceneReader; +import org.eclipse.paho.client.mqttv3.IMqttClient; +import org.eclipse.paho.client.mqttv3.MqttClient; +import org.eclipse.paho.client.mqttv3.MqttException; +import org.eclipse.paho.client.mqttv3.persist.MemoryPersistence; + +import java.io.IOException; +import java.util.List; +import java.util.UUID; public class SelectorNode extends DiNeRosDefaultNode { + private final String clientId = UUID.randomUUID().toString(); + public static IMqttClient client = null; + public static String mqttHost = null; + // mosquitto_pub -t 'Green' -m 'true' public SelectorNode(String nodeName, PetriNet petriNet, String rcHost) { super(nodeName, petriNet, rcHost, "mqtt"); + mqttHost = rcHost; + } + + @Override + protected TransitionSelectionResult onStartupEnded(List<Transition> list) { + + try { + System.out.println("[" + this.getDefaultNodeName() + "] Sending initial scene to webinterface via " + mqttHost); + + Scene scene = SceneReader.readFromJson("scenes/web_scene.json"); + client = new MqttClient("tcp://localhost:1883", clientId, new MemoryPersistence()); + MqttUtils.setupMqttOptions(client); + MqttUtils.sendScene("/main_controller/scene/update", scene, client); + } catch (MqttException | IOException e) { + e.printStackTrace(); + } + + return new FiringSelectionNone(); } } diff --git a/case-study-sorting-pkg/src/main/proto/Scene.proto b/case-study-sorting-pkg/src/main/proto/Scene.proto new file mode 100644 index 0000000000000000000000000000000000000000..2afd965f13a1fceddd00a2cfb66e748d0eca51ae --- /dev/null +++ b/case-study-sorting-pkg/src/main/proto/Scene.proto @@ -0,0 +1,137 @@ +// connector.proto +// this file contains the messages that are exchanged between the ROS connector and other systems + +syntax = "proto3"; + +option java_package = "de.tudresden.inf.st.ceti"; +option java_multiple_files = true; + +message Object { + + // Position is object-center related + message Position { + double x = 1; // in m + double y = 2; // in m + double z = 3; // height in m + } + + // 3D description of the object + message Size { + double length = 1; // in m + double width = 2; // in m + double height = 3; // in m + } + message Orientation { + double x = 1; // normalized quaternion + double y = 2; + double z = 3; + double w = 4; + } + message Color { + float r = 1; // 0..1 + float g = 2; // 0..1 + float b = 3; // 0..1 + } + enum Type { + UNKNOWN = 0; + BOX = 1; + BIN = 2; + ARM = 3; + DROP_OFF_LOCATION = 4; + HUMAN = 5; + ROBOT = 6; + COLLABORATION_ZONE = 7; + } + enum State { + STATE_UNKNOWN = 0; + + // robot states + STATE_IDLE = 101; + STATE_PICKING = 102; + STATE_PLACING = 103; + STATE_MOVING = 104; + + // object + STATE_STATIONARY = 200; + STATE_PICKED = 201; + } + + // determines the meaning of a scene object when the scene has is_delta flag + enum Mode { + MODE_UNKNOWN = 0; // if and only if is_delta = false, this should be the value of the field + MODE_KEEP = 1; // the object's properties do not change, thus don't need to be processed by the receiver + MODE_MODIFY = 2; // the object's properties have changed + MODE_ADD = 3; // the object did not exist in the scene before + MODE_REMOVE = 4; // the object is removed from the scene, its properties can be ignored + } + + string id = 1; + Type type = 2; + Position pos = 3; + Size size = 4; + Orientation orientation = 5; + Color color = 6; + State state = 7; + string owner = 8; + Mode mode = 9; +} + +// the scene is stored within the ROS side and sent to clients +message Scene { + repeated Object objects = 1; + bool is_delta = 2; +} + +// the selection is done by a client and sent to ROS +message Selection { + string id = 1; // the id corresponds to an id of an Object in a Scene +} + +message Command { + oneof msg { + PickAndPlace pickAndPlace = 1; + ConfigChange configChange = 2; + Evacuate evacuate = 3; + Pick pick = 4; + Place place = 5; + Drop drop = 6; + MoveToPose moveToPose = 7; + } +} + +message MoveToPose { + string idRobot = 1; + Object.Position position = 2; + Object.Orientation orientation = 3; +} + +message PickAndPlace { + string idRobot = 1; // id of the robot that should execute this operation + string idPick = 2; // id of the object in the scene to be picked + string idPlace = 3; // id of the location the picked object shall be placed. +} + +message ConfigChange { + string idCollaborationZone = 1; // id of collaboration zone to change + string idRobotNewOwner = 2; // id of robot that will become new owner +} + +message Evacuate { + string idRobot = 1; // id of robot that need to move out of its currently defined collision objects + string idCollaborationZone = 2; // id of collaboration zone to evacuate +} + +message Pick { + string idRobot = 1; // id of the robot that should execute this operation + string idPick = 2; // id of the object in the scene to be picked +} + +message Place { + string idRobot = 1; // id of the robot that should execute this operation + string idPlace = 2; // id of the location currently grasped object shall be placed. +} + +message Drop { + string idRobot = 1; // id of the robot that should execute this operation + string idBin = 2; // id of the bin the picked object shall be dropped in. +} \ No newline at end of file diff --git a/case-study-sorting-pkg/src/main/resources/scenes/web_scene.json b/case-study-sorting-pkg/src/main/resources/scenes/web_scene.json new file mode 100644 index 0000000000000000000000000000000000000000..0e596936d817483d8214aab91dcfa10361dc81af --- /dev/null +++ b/case-study-sorting-pkg/src/main/resources/scenes/web_scene.json @@ -0,0 +1,124 @@ +{ + "objects": [ + { + "id": "Blue-True", + "type": "BOX", + "pos": { + "x": 0.2, + "y": -0.3, + "z": 0.58 + }, + "size": { + "length": 0.1, + "width": 0.16, + "height": 0.16 + }, + "orientation": { + "w": 1 + }, + "color": { + "b": 0.5 + } + }, + { + "id": "Green-True", + "type": "BOX", + "pos": { + "x": 0.2, + "y": -0.1, + "z": 0.58 + }, + "size": { + "length": 0.1, + "width": 0.16, + "height": 0.16 + }, + "orientation": { + "w": 1 + }, + "color": { + "g": 0.5 + } + }, + { + "id": "Red-True", + "type": "BOX", + "pos": { + "x": 0.2, + "y": 0.1, + "z": 0.58 + }, + "size": { + "length": 0.1, + "width": 0.16, + "height": 0.16 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.5 + } + }, + { + "id": "Blue-False", + "type": "BOX", + "pos": { + "x": 0.4, + "y": -0.2, + "z": 0.54 + }, + "size": { + "length": 0.1, + "width": 0.08, + "height": 0.08 + }, + "orientation": { + "w": 1 + }, + "color": { + "b": 0.5 + } + }, + { + "id": "Green-False", + "type": "BOX", + "pos": { + "x": 0.4, + "y": 0, + "z": 0.54 + }, + "size": { + "length": 0.1, + "width": 0.08, + "height": 0.08 + }, + "orientation": { + "w": 1 + }, + "color": { + "g": 0.5 + } + }, + { + "id": "Red-False", + "type": "BOX", + "pos": { + "x": 0.4, + "y": 0.2, + "z": 0.54 + }, + "size": { + "length": 0.1, + "width": 0.08, + "height": 0.08 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.5 + } + } + ] +}