Skip to content
Snippets Groups Projects
Commit b6880ac9 authored by René Schöne's avatar René Schöne
Browse files

working on main case

- better reachability config
- working on pos calc
- add more demo commands (arm picking/placing)
parent 6246d60b
No related branches found
No related tags found
No related merge requests found
Showing
with 183 additions and 151 deletions
......@@ -22,7 +22,7 @@ public class Configuration {
}
public static class ConfigurationForB {
public List<String> reachability;
public String filenameReachability;
public List<String> topicsSceneUpdate;
public String topicCommand;
}
......
......@@ -99,6 +99,7 @@ public abstract class SharedMainParts<MqttHandler extends SharedMainParts.MqttHa
Runtime.getRuntime().addShutdownHook(new Thread(this::close));
exitCondition.await();
logger.debug("After exit condition");
}
public static String joinTopics(String... topics) {
......@@ -200,6 +201,7 @@ public abstract class SharedMainParts<MqttHandler extends SharedMainParts.MqttHa
logger.info("Exiting ...");
mainHandler.close();
model.ragconnectCloseConnections();
logger.debug("After closing connections");
}
public interface MqttHandlerWrapper<SELF> {
......
#mqttHost: "192.168.0.122"
mqttHost: "localhost"
mqttHost: "192.168.0.122"
#mqttHost: "localhost"
filenameRegions: "src/main/resources/regions-a-placeworld.json"
forA:
filenameInitialScene: "src/main/resources/config-scene-a-placeworld.json"
......
{ "objects": [
{ "id": "target","type": "DROP_OFF_LOCATION","pos": { "x": 8,"y": 3,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "r": 1 } },
{ "id": "bigBlue","type": "BOX","pos": { "x": 8,"y": 3,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "r": 1 } }
{ "id": "target_Blue","type": "DROP_OFF_LOCATION","pos": { "x": 8,"y": 3,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "r": 1 } },
{ "id": "target_Green","type": "DROP_OFF_LOCATION","pos": { "x": 1,"y": 1,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "r": 1 } },
{ "id": "bigBlue","type": "BOX","pos": { "x": 8,"y": 3,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "r": 1 } },
{ "id": "bigGreen","type": "BOX","pos": { "x": 1,"y": 1,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "r": 1 } }
] }
{
"regions": [
{ "name": "G", "positions": ["target"] }
{ "name": "G", "positions": ["target_Blue"] },
{ "name": "I", "positions": ["target_Green"] }
]
}
......@@ -309,32 +309,39 @@ aspect Computation {
syn String Robot.myPosition() {
//TODO "(^)" "(>)"
RobotObject myRobotObject = myRobotObject();
de.tudresden.inf.st.ceti.Object.State myState = myRobotObject != null ? myRobotObject.getState() : de.tudresden.inf.st.ceti.Object.State.STATE_UNKNOWN;
String pre = "eval of my pos for " + getName() + " .. current-pos: " + getCurrentPosition() + ", occ: '" + getOccupiedCollaborationZoneNames() + "', state: " + (myRobotObject != null ? myRobotObject.getState() : "null") + " -> ";
for (String zoneName : arrayAsList(getOccupiedCollaborationZoneNames())) {
if (zoneName.startsWith("(>)")) {
String targetZone = zoneName.substring(3);
if (getCurrentPosition().equals(zoneName) && myRobotObject.getState() == de.tudresden.inf.st.ceti.Object.State.STATE_IDLE) {
if (getCurrentPosition().equals(zoneName) && myState == de.tudresden.inf.st.ceti.Object.State.STATE_IDLE) {
System.out.println(pre + targetZone);
return targetZone; // robot was moving and has now reached its target
}
if (myRobotObject.getState() == de.tudresden.inf.st.ceti.Object.State.STATE_MOVING) {
if (isStateMoving(myState) || "unknown".equals(getCurrentPosition())) {
System.out.println(pre + zoneName);
return zoneName; // actually return (>) + targetZone
}
// robot is not moving (or picking?)
return targetZone;
}
if (zoneName.startsWith("(^)")) {
String targetZone = zoneName.substring(3);
if (getCurrentPosition().equals(zoneName) && myRobotObject.getState() == de.tudresden.inf.st.ceti.Object.State.STATE_IDLE) {
if (getCurrentPosition().equals(zoneName) && myState == de.tudresden.inf.st.ceti.Object.State.STATE_IDLE) {
System.out.println(pre + "(empty)");
return ""; // robot was moving and has now reached its target
}
if (myRobotObject.getState() == de.tudresden.inf.st.ceti.Object.State.STATE_MOVING) {
if (isStateMoving(myState) || "unknown".equals(getCurrentPosition())) {
System.out.println(pre + zoneName);
return zoneName; // actually return (^) + targetZone
}
// robot is not moving (or picking?)
return targetZone;
}
}
System.out.println(pre + getCurrentPosition());
return getCurrentPosition();
}
private boolean Robot.isStateMoving(de.tudresden.inf.st.ceti.Object.State state) {
return state == de.tudresden.inf.st.ceti.Object.State.STATE_MOVING || state == de.tudresden.inf.st.ceti.Object.State.STATE_PICKING || state == de.tudresden.inf.st.ceti.Object.State.STATE_PLACING;
}
}
aspect AttributeMappings {
......@@ -434,6 +441,9 @@ aspect Navigation {
}
syn boolean Robot.isBusy() {
if (getCurrentPosition().startsWith("(")) {
return true;
}
RobotObject obj = myRobotObject();
return obj != null && obj.getState() != de.tudresden.inf.st.ceti.Object.State.STATE_IDLE;
}
......
package de.tudresden.inf.st.placeB;
import de.tudresden.inf.st.ceti.Object;
import de.tudresden.inf.st.ceti.Reachability;
import de.tudresden.inf.st.placeB.ReachabilityConfiguration.RobotConfiguration;
import de.tudresden.inf.st.placeB.ast.*;
import de.tudresden.inf.st.ros3rag.common.Configuration;
import de.tudresden.inf.st.ros3rag.common.SharedMainParts;
import de.tudresden.inf.st.ros3rag.common.Util;
......@@ -45,7 +44,8 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> {
@Override
protected void createSpecificMainHandlerConnections() {
mainHandler.newConnection("place-b/demo", bytes -> {
switch (new String(bytes)) {
String topic = new String(bytes);
switch (topic) {
case "objectRed1/red":
UtilB.updatePositionOfObjectToLocation(model.getMyScene(), "objectRed1", "binRed");
break;
......@@ -57,17 +57,19 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> {
e.printStackTrace();
}
break;
case "arm1/moving":
updateAndPublishScene("arm1", robot -> robot.toBuilder().setState(Object.State.STATE_MOVING).build());
break;
case "arm1/idle":
updateAndPublishScene("arm1", robot -> robot.toBuilder().setState(Object.State.STATE_IDLE).build());
break;
case "arm2/moving":
updateAndPublishScene("arm2", robot -> robot.toBuilder().setState(Object.State.STATE_MOVING).build());
break;
case "arm1/picking":
case "arm1/placing":
case "arm1/moving":
case "arm2/idle":
updateAndPublishScene("arm2", robot -> robot.toBuilder().setState(Object.State.STATE_IDLE).build());
case "arm2/picking":
case "arm2/placing":
case "arm2/moving":
int slashIndex = topic.indexOf("/");
String robot = topic.substring(0, slashIndex);
String stateString = "STATE_" + topic.substring(slashIndex + 1).toUpperCase();
Object.State state = Object.State.valueOf(stateString);
updateAndPublishScene(robot, r -> r.toBuilder().setState(state).build());
break;
case "big-blue/cz":
updateAndPublishScene("bigBlue", bigBlue -> {
......@@ -90,7 +92,7 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> {
});
break;
default:
logger.error("Unknown demo command {}", new String(bytes));
logger.error("Unknown demo command {}", topic);
}
});
}
......@@ -116,15 +118,15 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> {
UtilB.setRegions(model, Util.parseRegionConfig(regionBFile));
// init robots and reachability
for (String reachabilityFileName : config.forB.reachability) {
// assumption: robots do not change during runtime, so we have stable connections
Path path = UtilB.pathToDirectoryOfPlaceB().resolve(Paths.get(reachabilityFileName));
Reachability reachability = UtilB.readReachability(path);
Path path = UtilB.pathToDirectoryOfPlaceB().resolve(Paths.get(config.forB.filenameReachability));
ReachabilityConfiguration reachability = UtilB.readReachability(path.toFile());
Robot robot = UtilB.createRobot(reachability.getIdRobot());
for (RobotConfiguration robotConfiguration : reachability.robots) {
Robot robot = UtilB.createRobot(robotConfiguration.name);
model.addRobot(robot);
CanReachObjectOfInterestWrapper reachabilityWrapper = UtilB.convert(reachability);
CanReachObjectOfInterestWrapper reachabilityWrapper = UtilB.convertToReachability(robotConfiguration.reachableLocations);
robot.setCanReachObjectOfInterestWrapper(reachabilityWrapper);
}
}
......@@ -132,18 +134,45 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> {
@Override
protected void connectEndpoints() throws IOException {
for (String topic : config.forB.topicsSceneUpdate) {
model.connectMyScene(mqttUri(topic, config));
}
model.connectOtherScene(mqttUri("place-a/logical/update", config), 0);
model.connectNextOperation(mqttUri(config.forB.topicCommand, config), false);
checkSuccess(
model.connectMyScene(mqttUri(topic, config)),
"connectMyScene"
);
}
checkSuccess(
model.connectOtherScene(mqttUri("place-a/logical/update", config), 0),
"OtherScene"
);
checkSuccess(
model.connectNextOperation(mqttUri(config.forB.topicCommand, config), false),
"NextOperation"
);
for (Robot robot : model.getRobotList()) {
// self-loop
robot.connectOwnedCollaborationZoneNames(mqttUri(config.forB.topicCommand, config));
robot.connectOccupiedCollaborationZoneNames(mqttUri(config.forB.topicCommand, config));
checkSuccess(
robot.connectOwnedCollaborationZoneNames(mqttUri(config.forB.topicCommand, config)),
"OwnedCollaborationZoneNames (" + robot.getName() + ")"
);
checkSuccess(
robot.connectOccupiedCollaborationZoneNames(mqttUri(config.forB.topicCommand, config)),
"OccupiedCollaborationZoneNames (" + robot.getName() + ")"
);
String topicPosition = joinTopics("place-b", robot.getName(), "position");
robot.connectCurrentPosition(mqttUri(topicPosition, config));
robot.connectMyPosition(mqttUri(topicPosition, config), true);
checkSuccess(
robot.connectCurrentPosition(mqttUri(topicPosition, config)),
"CurrentPosition (" + robot.getName() + ")"
);
checkSuccess(
robot.connectMyPosition(mqttUri(topicPosition, config), true),
"MyPosition (" + robot.getName() + ")"
);
}
}
private void checkSuccess(boolean connectSuccess, String target) {
if (!connectSuccess) {
logger.warn("Did not connect successfully to {}", target);
}
}
......@@ -154,7 +183,7 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> {
builder.excludeChildren("Orientation", "Size");
builder.excludeRelations("ContainedInRegion");
builder.includeNonterminalAttributes("LogicalScene", "diffScenes", "diffToOperations");
builder.includeAttributes("realRegion", "computeOperations");
builder.includeAttributes("realRegion", "computeOperations", "myPosition");
builder.includeNullNodes();
});
mainHandler.publish(TOPIC_MODEL_SVG_PATH, filename.getBytes(StandardCharsets.UTF_8));
......
package de.tudresden.inf.st.placeB;
import java.util.List;
/**
* Data class for reachability information.
*
* @author rschoene - Initial contribution
*/
public class ReachabilityConfiguration {
public List<RobotConfiguration> robots;
public static class RobotConfiguration {
public String name;
public List<String> reachableLocations;
}
}
......@@ -2,6 +2,7 @@ package de.tudresden.inf.st.placeB;
import de.tudresden.inf.st.ceti.Object;
import de.tudresden.inf.st.ceti.Reachability;
import de.tudresden.inf.st.placeB.ReachabilityConfiguration.RobotConfiguration;
import de.tudresden.inf.st.placeB.ast.*;
import de.tudresden.inf.st.ros3rag.common.Configuration;
import de.tudresden.inf.st.ros3rag.common.Util;
......@@ -13,7 +14,6 @@ import java.io.IOException;
import java.nio.charset.StandardCharsets;
import java.nio.file.Path;
import java.nio.file.Paths;
import java.util.Arrays;
import java.util.List;
import java.util.concurrent.CountDownLatch;
import java.util.concurrent.TimeUnit;
......@@ -80,10 +80,7 @@ public class SimpleMainB {
String filenameInitialSceneB = "src/main/resources/config-scene-b-" + scenario.suffix + ".json";
config.filenameRegions = "src/main/resources/regions-b-" + scenario.suffix + ".json";
config.forB = new Configuration.ConfigurationForB();
config.forB.reachability = Arrays.asList(
"src/main/resources/reachability-b-r1-" + scenario.suffix + ".json",
"src/main/resources/reachability-b-r2-" + scenario.suffix + ".json"
);
config.forB.filenameReachability = "src/main/resources/reachability-b-" + scenario.suffix + ".json";
Configuration configA = new Configuration();
String filenameInitialSceneA = "src/main/resources/config-scene-a-" + scenario.suffix + ".json";
......@@ -128,15 +125,15 @@ public class SimpleMainB {
UtilB.setRegions(model, Util.parseRegionConfig(regionBFile));
// init robots and reachability
for (String reachabilityFileName : config.forB.reachability) {
// assumption: robots do not change during runtime, so we have stable connections
Path path = UtilB.pathToDirectoryOfPlaceB().resolve(Paths.get(reachabilityFileName));
Reachability reachability = UtilB.readReachability(path);
Path path = UtilB.pathToDirectoryOfPlaceB().resolve(Paths.get(config.forB.filenameReachability));
ReachabilityConfiguration reachability = UtilB.readReachability(path.toFile());
for (RobotConfiguration robotConfiguration : reachability.robots) {
Robot robot = UtilB.createRobot(reachability.getIdRobot());
Robot robot = UtilB.createRobot(robotConfiguration.name);
model.addRobot(robot);
CanReachObjectOfInterestWrapper reachabilityWrapper = UtilB.convert(reachability);
CanReachObjectOfInterestWrapper reachabilityWrapper = UtilB.convertToReachability(robotConfiguration.reachableLocations);
robot.setCanReachObjectOfInterestWrapper(reachabilityWrapper);
}
......@@ -274,29 +271,6 @@ public class SimpleMainB {
}
@SuppressWarnings("unused")
private void testRobotReachabilityBFS() throws Exception {
WorldModelB model = new WorldModelB();
de.tudresden.inf.st.ceti.Scene scene = Util.readScene(
UtilB.pathToDirectoryOfPlaceB().resolve("src/main/resources/config-scene-b.json")
);
Scene myScene = UtilB.convert(scene);
model.setMyScene(myScene);
for (String name : Util.extractRobotNames(scene)) {
Robot robot = UtilB.createRobot(name);
model.addRobot(robot);
String filenameReachability = "src/main/resources/dummy-reachability-b-" + name + ".json";
Path path = UtilB.pathToDirectoryOfPlaceB().resolve(Paths.get(filenameReachability));
Reachability reachability = UtilB.readReachability(path);
CanReachObjectOfInterestWrapper reachabilityWrapper = UtilB.convert(reachability);
robot.setCanReachObjectOfInterestWrapper(reachabilityWrapper);
}
printReachability(model);
printShortestPath(model, "binRed", "binGreen");
printShortestPath(model, "binBlue", "binYellow");
printShortestPath(model, "binRed", "binPurple");
}
private void printReachability(WorldModelB model) {
System.out.println("ModelInfos:");
System.out.println(UtilB.getModelInfos(model, true));
......@@ -308,6 +282,7 @@ public class SimpleMainB {
System.out.println(model.toReachabilityGraph().prettyPrint());
}
@SuppressWarnings("unused")
private void printShortestPath(WorldModelB model, String source, String target) {
LogicalRegion sourceLocation = model.getMyScene().getLogicalScene()
.resolveLogicalObjectOfInterest(source).asLogicalRegion();
......@@ -446,25 +421,6 @@ public class SimpleMainB {
model.ragconnectCloseConnections();
}
@SuppressWarnings("unused")
private void testDummyReachability() throws Exception {
WorldModelB model = new WorldModelB();
Robot arm1 = UtilB.createRobot("ARM1");
model.addRobot(arm1);
Robot arm2 = UtilB.createRobot("ARM2");
model.addRobot(arm2);
Reachability reachabilityArm1 = UtilB.readReachability(UtilB.pathToDirectoryOfPlaceB()
.resolve("src/main/resources/dummy-reachability-b-arm1.json"));
Reachability reachabilityArm2 = UtilB.readReachability(UtilB.pathToDirectoryOfPlaceB()
.resolve("src/main/resources/dummy-reachability-b-arm2.json"));
CanReachObjectOfInterestWrapper canReachForArm1 = UtilB.convert(reachabilityArm1);
CanReachObjectOfInterestWrapper canReachForArm2 = UtilB.convert(reachabilityArm2);
arm1.setCanReachObjectOfInterestWrapper(canReachForArm1);
arm2.setCanReachObjectOfInterestWrapper(canReachForArm2);
printModelInfos(model);
}
@SuppressWarnings("unused")
private void testBuildModelB() {
WorldModelB model = new WorldModelB();
......
package de.tudresden.inf.st.placeB;
import com.fasterxml.jackson.databind.ObjectMapper;
import com.google.protobuf.util.JsonFormat;
import de.tudresden.inf.st.ceti.Object;
import de.tudresden.inf.st.ceti.Reachability;
......@@ -10,6 +11,7 @@ import de.tudresden.inf.st.ros3rag.common.Util;
import org.apache.logging.log4j.LogManager;
import org.apache.logging.log4j.Logger;
import java.io.File;
import java.io.IOException;
import java.nio.file.Files;
import java.nio.file.Path;
......@@ -24,12 +26,10 @@ import java.util.function.Function;
public class UtilB {
private static final Logger logger = LogManager.getLogger(UtilB.class);
static Reachability readReachability(Path path) throws IOException {
logger.debug("Reading reachability from {}", path.toAbsolutePath());
var jsonString = Files.readString(path);
var builder = de.tudresden.inf.st.ceti.Reachability.newBuilder();
JsonFormat.parser().merge(jsonString, builder);
return builder.build();
static ReachabilityConfiguration readReachability(File reachabilityFile) throws IOException {
logger.info("Using reachability config file: {}", reachabilityFile.getAbsolutePath());
ObjectMapper mapper = new ObjectMapper();
return mapper.readValue(reachabilityFile, ReachabilityConfiguration.class);
}
static Path pathToDirectoryOfPlaceB() {
......@@ -37,7 +37,7 @@ public class UtilB {
}
static Robot createRobot(String name, String... canReach) {
Robot result = new Robot().setName(name);
Robot result = new Robot().setName(name).setCurrentPosition("unknown");
CanReachObjectOfInterestWrapper wrapper = new CanReachObjectOfInterestWrapper();
for (String canReachName : canReach) {
......@@ -140,8 +140,10 @@ public class UtilB {
return new ExposingASTNode().exposed_apply_ConvertScene(scene);
}
static CanReachObjectOfInterestWrapper convert(de.tudresden.inf.st.ceti.Reachability r) throws Exception {
return new ExposingASTNode().exposed_apply_ConvertReachability(r);
static CanReachObjectOfInterestWrapper convertToReachability(List<String> reachableLocations) {
CanReachObjectOfInterestWrapper result = new CanReachObjectOfInterestWrapper();
reachableLocations.forEach(location -> result.addCanReachObjectOfInterest(new CanReachObjectOfInterest(location)));
return result;
}
static void setRegions(WorldModelB model, RegionConfiguration config) {
......
#mqttHost: "192.168.0.122"
mqttHost: "localhost"
mqttHost: "192.168.0.122"
#mqttHost: "localhost"
filenameRegions: "src/main/resources/regions-b-placeworld.json"
forB:
topicsSceneUpdate:
- "/ceti_cell_placeworld/scene/update"
- "/ceti_cell_2_placeworld/scene/update"
topicCommand: "/ceti_cell_placeworld/command"
reachability:
- "src/main/resources/reachability-b-arm1-placeworld.json"
- "src/main/resources/reachability-b-arm2-placeworld.json"
filenameReachability: "src/main/resources/reachability-b-placeworld.json"
coordinatorMqttTopicPrefix: "coordinating/rag-b"
{
"robots": [
{
"name": "R1",
"reachableLocations": [
"P1", "P3", "P5",
"P-E", "P-F"
]
},
{
"name": "R2",
"reachableLocations": [
"P2", "P4", "P6",
"P-E", "P-F"
]
}
]
}
reachability-b-arm1-placeworld.json
\ No newline at end of file
{
"idRobot": "arm1",
"objects": [
{ "idObject": "A1", "reachable": true },
{ "idObject": "B1", "reachable": true },
{ "idObject": "B2", "reachable": true },
{ "idObject": "C1", "reachable": true },
{ "idObject": "cz1", "reachable": true },
{ "idObject": "G1", "reachable": false }
]
}
reachability-b-arm2-placeworld.json
\ No newline at end of file
{
"idRobot": "arm2",
"objects": [
{ "idObject": "A1", "reachable": false },
{ "idObject": "B1", "reachable": false },
{ "idObject": "B2", "reachable": false },
{ "idObject": "C1", "reachable": false },
{ "idObject": "cz1", "reachable": true },
{ "idObject": "G1", "reachable": true }
]
}
{
"robots": [
{
"name": "R1",
"reachableLocations": [
"P1",
"P-E"
]
},
{
"name": "R2",
"reachableLocations": [
"P2.1", "P2.2",
"P-E"
]
}
]
}
reachability-b-placeworld.json
\ No newline at end of file
{
"robots": [
{
"name": "arm1",
"reachableLocations": [
"A1",
"B1", "B2",
"C1",
"cz1"
]
},
{
"name": "arm2",
"reachableLocations": [
"cz1",
"G1",
"I1", "I2", "I3" ,"I4"
]
}
]
}
{
"idRobot": "R1",
"objects": [
{ "idObject": "O1", "reachable": true },
{ "idObject": "O2", "reachable": false },
{ "idObject": "O3", "reachable": true },
{ "idObject": "O4-uninvolved", "reachable": true },
{ "idObject": "O5-uninvolved", "reachable": true },
{ "idObject": "O6-uninvolved", "reachable": false },
{ "idObject": "P1", "reachable": true },
{ "idObject": "P2", "reachable": false },
{ "idObject": "P3", "reachable": true },
{ "idObject": "P4", "reachable": false },
{ "idObject": "P5", "reachable": true },
{ "idObject": "P6", "reachable": false },
{ "idObject": "P-E", "reachable": true },
{ "idObject": "P-F", "reachable": true }
]
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment