Skip to content
Snippets Groups Projects
Commit 283bc524 authored by Sebastian Ebert's avatar Sebastian Ebert
Browse files

fixed bugs related to placing

parent 2ff702de
No related branches found
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
Showing
with 137 additions and 62 deletions
case-study-sorting-pkg/src/main/java/de/tudresden/inf/st/sorting/handlers/ControlEndHandler.java
+ 9
− 19
View file @ 283bc524
......@@ -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
+ 6
− 1
View file @ 283bc524
......@@ -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
+ 27
− 1
View file @ 283bc524
......@@ -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
+ 6
− 3
View file @ 283bc524
......@@ -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
+ 64
− 35
View file @ 283bc524
......@@ -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
+ 11
− 0
View file @ 283bc524
......@@ -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
+ 11
− 0
View file @ 283bc524
......@@ -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
+ 3
− 3
View file @ 283bc524
......@@ -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>
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment