From 0e2c0f00bf3374dac8f3c06376d265aa64b12cf6 Mon Sep 17 00:00:00 2001
From: rschoene <rene.schoene@tu-dresden.de>
Date: Thu, 5 May 2022 18:16:06 +0200
Subject: [PATCH] working on real collaboration
- update config structure
- fixed exit and rewind
- reread config on rewind
- read regions on both sites
- add demo commands for site B
- create svg at model-command for site-B, and publish filename
- enhance compute-operations: source location can be collaboration zone as well, add wait command when zone is not owned and occupied
- connect occupiedCollaborationZoneNames with self-loop and "moving-in" and "moving-out" logic
---
.../inf/st/ros3rag/common/Configuration.java | 14 ++-
.../st/ros3rag/common/SharedMainParts.java | 22 +++--
.../de/tudresden/inf/st/placeA/MainA.java | 10 +-
.../de/tudresden/inf/st/placeA/UtilA.java | 13 +++
.../main/resources/config-a-placeworld.yaml | 5 +
.../src/main/jastadd/WorldModelB.connect | 71 ++++++++++++++-
.../src/main/jastadd/WorldModelB.jadd | 66 +++++++++++---
.../src/main/jastadd/WorldModelB.relast | 4 +-
.../de/tudresden/inf/st/placeB/MainB.java | 91 ++++++++++++++++---
.../tudresden/inf/st/placeB/SimpleMainB.java | 72 +++++----------
.../de/tudresden/inf/st/placeB/UtilB.java | 27 +++++-
.../main/resources/config-b-placeworld.yaml | 15 ++-
.../main/resources/regions-b-placeworld.json | 3 +-
13 files changed, 317 insertions(+), 96 deletions(-)
create mode 100644 ros3rag.placeA/src/main/resources/config-a-placeworld.yaml
diff --git a/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/Configuration.java b/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/Configuration.java
index daf8b29..ee6c6f5 100644
--- a/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/Configuration.java
+++ b/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/Configuration.java
@@ -12,13 +12,19 @@ import java.util.List;
@JsonIgnoreProperties(ignoreUnknown = true)
public class Configuration {
public String mqttHost;
- public String filenameInitialSceneA;
public String filenameRegions;
public String coordinatorMqttTopicPrefix;
- public List<ReachabilityConfig> reachability;
+ public ConfigurationForA forA;
+ public ConfigurationForB forB;
- public static class ReachabilityConfig {
- public String filename;
+ public static class ConfigurationForA {
+ public String filenameInitialScene;
+ }
+
+ public static class ConfigurationForB {
+ public List<String> reachability;
+ public List<String> topicsSceneUpdate;
+ public String topicCommand;
}
public boolean useCoordinator() {
diff --git a/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/SharedMainParts.java b/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/SharedMainParts.java
index 0204821..89e81de 100644
--- a/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/SharedMainParts.java
+++ b/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/SharedMainParts.java
@@ -65,18 +65,21 @@ public abstract class SharedMainParts<MqttHandler extends SharedMainParts.MqttHa
mainHandler.newConnection(TOPIC_EXIT, bytes -> {
logger.info("Got exit command");
exitCondition.countDown();
+ startCondition.countDown();
logger.debug("exit latch count = {}, start latch count= {}",
exitCondition.getCount(), startCondition.getCount());
});
mainHandler.newConnection(TOPIC_MODEL, bytes -> logStatus(new String(bytes)));
mainHandler.newConnection(TOPIC_REWIND, bytes ->
- {
- try {
- rewind("rewind");
- } catch (Exception e) {
- logger.catching(e);
+ new Thread(() -> {
+ {
+ try {
+ rewind("rewind");
+ } catch (Exception e) {
+ logger.catching(e);
+ }
}
- }
+ }).start()
);
if (config.useCoordinator()) {
logger.info("Using coordinator logic");
@@ -136,11 +139,14 @@ public abstract class SharedMainParts<MqttHandler extends SharedMainParts.MqttHa
logger.debug("Closing previous connections for {}", cellName);
model.ragconnectCloseConnections();
}
+
+ config = Util.parseConfig(pathToConfig.toFile());
+
logger.debug("Creating world model for {}", cellName);
model = createWorldModel();
logger.debug("Reading robots for {}", cellName);
- readRobotsAndReachability();
+ readInitialConfigs();
logger.debug("Setup model connection for {}", cellName);
model.ragconnectCheckIncremental();
@@ -172,7 +178,7 @@ public abstract class SharedMainParts<MqttHandler extends SharedMainParts.MqttHa
protected abstract WorldModel createWorldModel() throws Exception;
- protected abstract void readRobotsAndReachability() throws Exception;
+ protected abstract void readInitialConfigs() throws Exception;
protected abstract void connectEndpoints() throws IOException;
diff --git a/ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/MainA.java b/ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/MainA.java
index 2dfe785..a0dc2e0 100644
--- a/ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/MainA.java
+++ b/ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/MainA.java
@@ -4,7 +4,9 @@ import de.tudresden.inf.st.placeA.ast.MqttHandler;
import de.tudresden.inf.st.placeA.ast.Scene;
import de.tudresden.inf.st.placeA.ast.WorldModelA;
import de.tudresden.inf.st.ros3rag.common.SharedMainParts;
+import de.tudresden.inf.st.ros3rag.common.Util;
+import java.io.File;
import java.io.IOException;
import static de.tudresden.inf.st.ros3rag.common.Util.mqttUri;
@@ -62,14 +64,18 @@ public class MainA extends SharedMainParts<MqttHandler, WorldModelA> {
@Override
protected WorldModelA createWorldModel() throws Exception {
de.tudresden.inf.st.ceti.Scene scene = readScene(
- UtilA.pathToDirectoryOfPlaceA().resolve(config.filenameInitialSceneA)
+ UtilA.pathToDirectoryOfPlaceA().resolve(config.forA.filenameInitialScene)
);
Scene myScene = UtilA.convert(scene);
return new WorldModelA().setScene(myScene);
}
@Override
- protected void readRobotsAndReachability() {
+ protected void readInitialConfigs() throws IOException {
+ // read and set regions
+ File regionFile = UtilA.pathToDirectoryOfPlaceA().resolve(config.filenameRegions).toFile();
+ UtilA.setRegions(model, Util.parseRegionConfig(regionFile));
+
// no robots to be set
}
diff --git a/ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/UtilA.java b/ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/UtilA.java
index 405f67a..0fea7e2 100644
--- a/ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/UtilA.java
+++ b/ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/UtilA.java
@@ -1,6 +1,8 @@
package de.tudresden.inf.st.placeA;
import de.tudresden.inf.st.placeA.ast.*;
+import de.tudresden.inf.st.ros3rag.common.RegionConfiguration;
+import de.tudresden.inf.st.ros3rag.common.RegionConfiguration.RegionDefinition;
import de.tudresden.inf.st.ros3rag.common.Util;
import org.apache.logging.log4j.LogManager;
import org.apache.logging.log4j.Logger;
@@ -23,6 +25,17 @@ public class UtilA {
return new ExposingASTNode().exposed_apply_ConvertScene(scene);
}
+ static void setRegions(WorldModelA model, RegionConfiguration config) {
+ JastAddList<Region> result = new JastAddList<>();
+ for (RegionDefinition def : config.regions) {
+ Region region = new Region();
+ region.setName(def.name);
+ region.setLocationNames(String.join(",", def.positions));
+ result.add(region);
+ }
+ model.setRegionList(result);
+ }
+
static void updatePositionOfObjectToLocation(Scene scene, String objName, String locationName) {
ObjectOfInterest obj = scene.resolveObjectOfInterest(objName);
ObjectOfInterest location = scene.resolveObjectOfInterest(locationName);
diff --git a/ros3rag.placeA/src/main/resources/config-a-placeworld.yaml b/ros3rag.placeA/src/main/resources/config-a-placeworld.yaml
new file mode 100644
index 0000000..19e4029
--- /dev/null
+++ b/ros3rag.placeA/src/main/resources/config-a-placeworld.yaml
@@ -0,0 +1,5 @@
+mqttHost: "192.168.0.122"
+filenameRegions: "src/main/resources/regions-a-placeworld.json"
+forA:
+ filenameInitialScene: "src/main/resources/config-scene-a-placeworld.json"
+coordinatorMqttTopicPrefix: "coordinating/rag-a"
diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.connect b/ros3rag.placeB/src/main/jastadd/WorldModelB.connect
index 2ef0239..de33b2f 100644
--- a/ros3rag.placeB/src/main/jastadd/WorldModelB.connect
+++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.connect
@@ -4,6 +4,7 @@ receive indexed WorldModelB.OtherScene ;
receive WorldModelB.TestingOtherScene ;
receive Robot.CanReachObjectOfInterestWrapper using ParseReachability, ConvertReachability ;
receive Robot.OwnedCollaborationZoneNames using ConfigChangeCommandCheckForOwnedCollaborationZone ;
+receive Robot.OccupiedCollaborationZoneNames using CommandCheckForOccupiedCollaborationZone ;
// --- sending ---
send WorldModelB.NextOperation using PrintOperation ;
@@ -27,7 +28,7 @@ ConfigChangeCommandCheckForOwnedCollaborationZone maps byte[] bytes to String {:
if (!command.hasConfigChange()) {
reject();
}
- System.out.println(command);
+// System.out.println(command);
de.tudresden.inf.st.ceti.ConfigChange cc = command.getConfigChange();
Scene scene = worldModelB().getMyScene();
ObjectOfInterest obj = scene.resolveObjectOfInterest(cc.getIdCollaborationZone());
@@ -49,3 +50,71 @@ ConfigChangeCommandCheckForOwnedCollaborationZone maps byte[] bytes to String {:
}
return String.join(",", collaborationZoneNames);
:}
+
+CommandCheckForOccupiedCollaborationZone maps byte[] bytes to String {:
+ de.tudresden.inf.st.ceti.Command command = null;
+ try {
+ command = de.tudresden.inf.st.ceti.Command.parseFrom(bytes);
+ } catch (com.google.protobuf.InvalidProtocolBufferException e) {
+ reject();
+ }
+ // if the command is not the correct command type, do not change anything
+ if (!command.hasPickAndPlace() && !command.hasEvacuate()) {
+ reject();
+ }
+// System.out.println(command);
+
+ Scene scene = worldModelB().getMyScene();
+ Robot thisRobot = (Robot) this;
+
+// // get current value, but remove every "moving out" zone
+// List<String> occupiedZoneNames = thisRobot.occupiedCollaborationZonesAsList().stream()
+// .filter(!zone -> zone.startsWith("(^)"))
+// .collect(java.util.stream.Collectors.toList());
+
+ // with a new command, begin with a clean occ list
+ List<String> occupiedZoneNames = new java.util.ArrayList<>();
+
+ // (>) == moving in, (^) == evacuate/moving out
+ boolean updated = false;
+ if (command.hasPickAndPlace()) {
+ de.tudresden.inf.st.ceti.PickAndPlace pp = command.getPickAndPlace();
+
+ // if the command is not executed by this robot, do not change anything
+ if (!thisRobot.getName().equals(pp.getIdRobot())) {
+ reject();
+ }
+
+ ObjectOfInterest obj = scene.resolveObjectOfInterest(pp.getIdPlace());
+ DropOffLocation loc = obj.asDropOffLocation();
+ if (loc.isCollaborationZone()) {
+ CollaborationZone cz = loc.asCollaborationZone();
+ String specialIncomingName = "(>)" + cz.getName();
+ if (!occupiedZoneNames.contains(specialIncomingName)) {
+ occupiedZoneNames.add(specialIncomingName);
+ updated = true;
+ }
+ }
+ } else if (command.hasEvacuate()) {
+ de.tudresden.inf.st.ceti.Evacuate ev = command.getEvacuate();
+
+ // if the command is not executed by this robot, do not change anything
+ if (!thisRobot.getName().equals(ev.getIdRobot())) {
+ reject();
+ }
+ ObjectOfInterest obj = scene.resolveObjectOfInterest(ev.getIdCollaborationZone());
+ DropOffLocation loc = obj.asDropOffLocation();
+ if (loc.isCollaborationZone()) {
+ CollaborationZone cz = loc.asCollaborationZone();
+ String specialEvacuateName = "(^)" + cz.getName();
+ if (!occupiedZoneNames.contains(specialEvacuateName)) {
+ occupiedZoneNames.add(specialEvacuateName);
+ updated = true;
+ }
+ }
+ }
+ if (!updated) {
+ reject();
+ }
+ return String.join(",", occupiedZoneNames);
+:}
diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd
index 0980bd0..6142c56 100644
--- a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd
+++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd
@@ -165,22 +165,43 @@ aspect Computation {
Vertex transit = previousVertex;
for (Edge edge : shortestPath) {
Vertex target = edge.getFrom().equals(transit) ? edge.getTo() : edge.getFrom();
+ DropOffLocation sourceLocation = worldModelB().getMyScene().resolveObjectOfInterest(previousVertex.asVertexWithObjectOfInterest().getNameOfObjectOfInterest()).asDropOffLocation();
DropOffLocation targetLocation = worldModelB().getMyScene().resolveObjectOfInterest(target.asVertexWithObjectOfInterest().getNameOfObjectOfInterest()).asDropOffLocation();
// Robot executingRobot = worldModelB().findRobot(edge.asEdgeWithRobot().getNameOfRobot());
worldModelB().findRobot(edge.asEdgeWithRobot().getNameOfRobot()).ifPresentOrElse(executingRobot -> {
+ /* for both locations, in case it is a collaboration zone, we need to ensure the following:
+ (a) claimed by the executing robot, and
+ (b) not occupied by another robot, and
+ (c) free (as no movable object is contained) -- this should be reflected in the graph!
+ */
+ if (sourceLocation.isCollaborationZone()) {
+ CollaborationZone cz = sourceLocation.asCollaborationZone();
+ System.out.println("source is cz, occ:" + (cz.hasOccupient() ? cz.occupient().nameAndHash() : "none") + ", owner: " + (cz.hasOwner() ? cz.owner().nameAndHash() : "none"));
+ // order is important here, first add Evacuate, then ConfigChange
+ if (cz.hasOccupient() && !cz.occupient().equals(executingRobot)) {
+ result.add(Evacuate.of(cz.occupient(), cz));
+ }
+ if (!cz.hasOwner() || (cz.hasOwner() && !cz.owner().equals(executingRobot))) {
+ if (cz.hasOccupient() && !cz.occupient().equals(executingRobot)) {
+ result.add(Wait.of());
+ } else {
+ result.add(ConfigChange.of(executingRobot, cz));
+ }
+ }
+ }
if (targetLocation.isCollaborationZone()) {
- /* here we need to check, if the collaboration zone is
- (a) claimed by the executing robot, and
- (b) occupied by another robot, and
- (c) free (as no movable object is contained) -- this should be reflected in the graph!
- */
CollaborationZone cz = targetLocation.asCollaborationZone();
+ System.out.println("target is cz, occ:" + (cz.hasOccupient() ? cz.occupient().nameAndHash() : "none") + ", owner: " + (cz.hasOwner() ? cz.owner().nameAndHash() : "none"));
// order is important here, first add Evacuate, then ConfigChange
if (cz.hasOccupient() && !cz.occupient().equals(executingRobot)) {
result.add(Evacuate.of(cz.occupient(), cz));
}
if (!cz.hasOwner() || (cz.hasOwner() && !cz.owner().equals(executingRobot))) {
- result.add(ConfigChange.of(executingRobot, cz));
+ if (cz.hasOccupient() && !cz.occupient().equals(executingRobot)) {
+ result.add(Wait.of());
+ } else {
+ result.add(ConfigChange.of(executingRobot, cz));
+ }
}
}
result.add(PickAndPlace.of(executingRobot, getObject(), targetLocation));
@@ -229,6 +250,10 @@ aspect Computation {
return result;
}
+ public static Wait Wait.of() {
+ return new Wait();
+ }
+
//--- getNextOperation ---
syn Operation WorldModelB.getNextOperation() {
if (diffToOperations().getNumChild() == 0) {
@@ -306,6 +331,7 @@ aspect AttributeMappings {
.setIdCollaborationZone(getCollaborationZone().getName())
.build())
.build().toByteArray();
+ eq Wait.toProtobufByteArray() = null;
}
aspect Navigation {
@@ -323,9 +349,20 @@ aspect Navigation {
// eq WorldModelB.getChild().allMappings() = getLocationMapping();
syn boolean CollaborationZone.hasOccupient() = occupient() != null;
syn Robot CollaborationZone.occupient() {
+ // (>) == moving in, (^) == evacuate/moving out
for (Robot robot : worldModelB().getRobotList()) {
- if (getName().equals(robot.getOccupiedCollaborationZoneName())) {
- return robot;
+ for (String zoneName : arrayAsList(robot.getOccupiedCollaborationZoneNames())) {
+ if (zoneName.startsWith("(>)") && zoneName.substring(3).equals(getName())) {
+ // robot is currently moving into this zone. always regard it as occupied by it
+ return robot;
+ }
+ if (zoneName.startsWith("(^)") && zoneName.substring(3).equals(getName()) && robot.isBusy()) {
+ // robot is busy moving out of this zone. regard it as occupied as long as the robot is busy
+ return robot;
+ }
+ if (zoneName.equals(getName())) {
+ return robot;
+ }
}
}
return null;
@@ -333,7 +370,7 @@ aspect Navigation {
syn boolean CollaborationZone.hasOwner() = owner() != null;
syn Robot CollaborationZone.owner() {
for (Robot robot : worldModelB().getRobotList()) {
- List<String> collaborationZoneNames = Arrays.asList(robot.getOwnedCollaborationZoneNames().split(","));
+ List<String> collaborationZoneNames = arrayAsList(robot.getOwnedCollaborationZoneNames());
if (collaborationZoneNames.contains(getName())) {
return robot;
}
@@ -341,6 +378,7 @@ aspect Navigation {
return null;
}
syn List<String> Robot.ownedCollaborationZonesAsList() = arrayAsList(getOwnedCollaborationZoneNames());
+ syn List<String> Robot.occupiedCollaborationZonesAsList() = arrayAsList(getOccupiedCollaborationZoneNames());
eq Region.locationList() {
List<DropOffLocation> result = new ArrayList<>();
if (!worldModelB().hasMyScene()) { return result; }
@@ -398,18 +436,24 @@ aspect Printing {
eq Evacuate.prettyPrint() {
return "+Evacuate: " + getRobotToExecute().prettyPrint() + " from " + getCollaborationZone().prettyPrint() + "+";
}
+ eq Wait.prettyPrint() {
+ return "+Wait+";
+ }
}
aspect DumpAst {
- public void WorldModelB.dumpAst(java.util.function.Consumer<de.tudresden.inf.st.jastadd.dumpAst.ast.DumpBuilder> options) {
+ public String WorldModelB.dumpAst(java.util.function.Consumer<de.tudresden.inf.st.jastadd.dumpAst.ast.DumpBuilder> options) {
String now = java.time.LocalDateTime.now().format(java.time.format.DateTimeFormatter.ISO_LOCAL_DATE_TIME).replaceAll("[:\\.]", "-");
try {
de.tudresden.inf.st.jastadd.dumpAst.ast.DumpBuilder builder = de.tudresden.inf.st.jastadd.dumpAst.ast.Dumper.read(this);
options.accept(builder);
//builder.dumpAsPNG(java.nio.file.Paths.get("images/" + now + ".png"));
- builder.dumpAsSVG(java.nio.file.Paths.get("images/" + now + ".svg"));
+ java.nio.file.Path path = java.nio.file.Paths.get("images/" + now + ".svg");
+ builder.dumpAsSVG(path);
+ return path.getFileName().toString();
} catch (java.io.IOException e) {
e.printStackTrace();
+ return null;
}
}
}
diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.relast b/ros3rag.placeB/src/main/jastadd/WorldModelB.relast
index e6bb98b..7dc16b5 100644
--- a/ros3rag.placeB/src/main/jastadd/WorldModelB.relast
+++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.relast
@@ -1,7 +1,7 @@
WorldModelB ::= Region* Robot* [MyScene:Scene] OtherScene:LogicalScene* /NextOperation:Operation/ TestingOtherScene:LogicalScene* ;
// FIXME inline CanReachObjectOfInterestWrapper
-Robot ::= <Name:String> CanReachObjectOfInterestWrapper <OwnedCollaborationZoneNames> <OccupiedCollaborationZoneName> ;
+Robot ::= <Name:String> CanReachObjectOfInterestWrapper <OwnedCollaborationZoneNames> <OccupiedCollaborationZoneNames> ;
// relations into nodes received by RagConnect are not allowed
//rel Robot.OwnedCollaborationZone* <-> CollaborationZone.Owner? ;
//rel Robot.OccupiedCollaborationZone? <-> CollaborationZone.Occupient? ;
@@ -34,3 +34,5 @@ rel ConfigChange.CollaborationZone -> CollaborationZone ;
Evacuate : Operation ;
rel Evacuate.CollaborationZone -> CollaborationZone ;
+
+Wait : Operation ;
diff --git a/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/MainB.java b/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/MainB.java
index 4095335..027516d 100644
--- a/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/MainB.java
+++ b/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/MainB.java
@@ -1,13 +1,18 @@
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.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;
+import java.io.File;
import java.io.IOException;
+import java.nio.charset.StandardCharsets;
import java.nio.file.Path;
import java.nio.file.Paths;
+import java.util.function.Function;
import static de.tudresden.inf.st.ros3rag.common.Util.mqttUri;
@@ -17,19 +22,16 @@ import static de.tudresden.inf.st.ros3rag.common.Util.mqttUri;
* @author rschoene - Initial contribution
*/
public class MainB extends SharedMainParts<MqttHandler, WorldModelB> {
- private final String TOPIC_MY_SCENE_UPDATE_FROM_ROS;
- private final String TOPIC_COMMAND;
- private final String TOPIC_OTHER_SCENE_UPDATE_FROM_PLACE_A;
private final String TOPIC_DEMO_MOVE_objectRed1_RED;
+ private final String TOPIC_MODEL_SVG_PATH;
+
+ private de.tudresden.inf.st.ceti.Scene demo_scene;
MainB(String configFile) {
super("place-b", UtilB.pathToDirectoryOfPlaceB().resolve(configFile));
- this.TOPIC_MY_SCENE_UPDATE_FROM_ROS = cellName + "/scene/update";
- this.TOPIC_COMMAND = cellName + "/command";
-
- this.TOPIC_OTHER_SCENE_UPDATE_FROM_PLACE_A = "place-a/logical/update";
this.TOPIC_DEMO_MOVE_objectRed1_RED = cellName + "/demo/move/objectRed1/red";
+ this.TOPIC_MODEL_SVG_PATH = cellName + "/model/svg/path";
}
public static void main(String[] args) throws Exception {
@@ -47,6 +49,47 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> {
mainHandler.newConnection(TOPIC_DEMO_MOVE_objectRed1_RED, bytes ->
UtilB.updatePositionOfObjectToLocation(model.getMyScene(), "objectRed1", "binRed")
);
+ mainHandler.newConnection("demo/initial_scene", bytes -> {
+ try {
+ demo_scene = Util.readScene(UtilB.pathToDirectoryOfPlaceB().resolve("src/main/resources/config-scene-b-placeworld-manual.json"));
+ mainHandler.publish(config.forB.topicsSceneUpdate.get(0), demo_scene.toByteArray());
+ } catch (IOException e) {
+ e.printStackTrace();
+ }
+ });
+ mainHandler.newConnection("demo/arm1/active", bytes ->
+ updateAndPublishScene("arm1", robot -> robot.toBuilder().setActive(true).build()));
+ mainHandler.newConnection("demo/arm1/inactive", bytes ->
+ updateAndPublishScene("arm1", robot -> robot.toBuilder().setActive(false).build()));
+ mainHandler.newConnection("demo/arm2/active", bytes ->
+ updateAndPublishScene("arm2", robot -> robot.toBuilder().setActive(true).build()));
+ mainHandler.newConnection("demo/arm2/inactive", bytes ->
+ updateAndPublishScene("arm2", robot -> robot.toBuilder().setActive(false).build()));
+ mainHandler.newConnection("demo/big-blue/cz", bytes ->
+ updateAndPublishScene("bigBlue", bigBlue -> {
+ Object.Builder builder = bigBlue.toBuilder();
+ builder.getPosBuilder()
+ .setX(0.0012)
+ .setY(0.65)
+ .setZ(0.94);
+ return builder.build();
+ })
+ );
+ mainHandler.newConnection("demo/big-blue/g1", bytes ->
+ updateAndPublishScene("bigBlue", bigBlue -> {
+ Object.Builder builder = bigBlue.toBuilder();
+ builder.getPosBuilder()
+ .setX(0.1)
+ .setY(1.0)
+ .setZ(0.93);
+ return builder.build();
+ })
+ );
+ }
+
+ private void updateAndPublishScene(String objectName, Function<Object, Object> change) {
+ UtilB.updateObject(demo_scene, objectName, change);
+ mainHandler.publish(config.forB.topicsSceneUpdate.get(0), demo_scene.toByteArray());
}
@Override
@@ -57,13 +100,17 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> {
}
@Override
- protected void readRobotsAndReachability() throws Exception {
+ protected void readInitialConfigs() throws Exception {
model.setMyScene(new Scene());
+ // read and set regions
+ File regionBFile = UtilB.pathToDirectoryOfPlaceB().resolve(config.filenameRegions).toFile();
+ UtilB.setRegions(model, Util.parseRegionConfig(regionBFile));
+
// init robots and reachability
- for (Configuration.ReachabilityConfig reachabilityConfig : config.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(reachabilityConfig.filename));
+ Path path = UtilB.pathToDirectoryOfPlaceB().resolve(Paths.get(reachabilityFileName));
Reachability reachability = UtilB.readReachability(path);
Robot robot = UtilB.createRobot(reachability.getIdRobot());
@@ -76,17 +123,33 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> {
@Override
protected void connectEndpoints() throws IOException {
- model.connectMyScene(mqttUri(TOPIC_MY_SCENE_UPDATE_FROM_ROS, config));
- model.connectOtherScene(mqttUri(TOPIC_OTHER_SCENE_UPDATE_FROM_PLACE_A, config), 0);
- model.connectNextOperation(mqttUri(TOPIC_COMMAND, config), false);
+ 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);
for (Robot robot : model.getRobotList()) {
// self-loop
- robot.connectOwnedCollaborationZoneNames(mqttUri(TOPIC_COMMAND, config));
+ robot.connectOwnedCollaborationZoneNames(mqttUri(config.forB.topicCommand, config));
+ robot.connectOccupiedCollaborationZoneNames(mqttUri(config.forB.topicCommand, config));
}
}
@Override
protected String getModelInfos(WorldModelB worldModelB, boolean detailed) {
+ try {
+ String filename = model.dumpAst(builder -> {
+ builder.excludeChildren("Orientation", "Size");
+ builder.excludeRelations("ContainedInRegion");
+ builder.includeNonterminalAttributes("LogicalScene", "diffScenes", "diffToOperations");
+ builder.includeAttributes("realRegion", "computeOperations");
+ builder.includeNullNodes();
+ });
+ mainHandler.publish(TOPIC_MODEL_SVG_PATH, filename.getBytes(StandardCharsets.UTF_8));
+ } catch (Exception e) {
+ logger.catching(e);
+ }
+
return UtilB.getModelInfos(model, detailed);
}
diff --git a/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/SimpleMainB.java b/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/SimpleMainB.java
index 41630be..8021b43 100644
--- a/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/SimpleMainB.java
+++ b/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/SimpleMainB.java
@@ -4,7 +4,6 @@ import de.tudresden.inf.st.ceti.Object;
import de.tudresden.inf.st.ceti.Reachability;
import de.tudresden.inf.st.placeB.ast.*;
import de.tudresden.inf.st.ros3rag.common.Configuration;
-import de.tudresden.inf.st.ros3rag.common.Configuration.ReachabilityConfig;
import de.tudresden.inf.st.ros3rag.common.Util;
import org.apache.logging.log4j.LogManager;
import org.apache.logging.log4j.Logger;
@@ -18,7 +17,6 @@ import java.util.Arrays;
import java.util.List;
import java.util.concurrent.CountDownLatch;
import java.util.concurrent.TimeUnit;
-import java.util.function.Function;
import java.util.stream.Collectors;
/**
@@ -64,7 +62,7 @@ public class SimpleMainB {
@SuppressWarnings("unused" )
Scenario[] allScenarios = new Scenario[] { s2022, sMini, sPlaceworld, sPlaceworldManual };
- final Scenario scenario = sPlaceworldManual;
+ final Scenario scenario = sPlaceworld;
public static void main(String[] args) throws Exception {
System.out.println("Running SimpleMainB");
@@ -81,11 +79,10 @@ public class SimpleMainB {
config.mqttHost = scenario.mqttHost;
String filenameInitialSceneB = "src/main/resources/config-scene-b-" + scenario.suffix + ".json";
config.filenameRegions = "src/main/resources/regions-b-" + scenario.suffix + ".json";
- ReachabilityConfig reachabilityR1 = new ReachabilityConfig();
- reachabilityR1.filename = "src/main/resources/reachability-b-r1-" + scenario.suffix + ".json";
- ReachabilityConfig reachabilityR2 = new ReachabilityConfig();
- reachabilityR2.filename = "src/main/resources/reachability-b-r2-" + scenario.suffix + ".json";
- config.reachability = Arrays.asList(reachabilityR1, reachabilityR2);
+ config.forB.reachability = Arrays.asList(
+ "src/main/resources/reachability-b-r1-" + scenario.suffix + ".json",
+ "src/main/resources/reachability-b-r2-" + scenario.suffix + ".json"
+ );
Configuration configA = new Configuration();
String filenameInitialSceneA = "src/main/resources/config-scene-a-" + scenario.suffix + ".json";
@@ -108,20 +105,20 @@ public class SimpleMainB {
mqttHandler.waitUntilReady(2, TimeUnit.SECONDS);
model.ragconnectSetupMqttWaitUntilReady(2, TimeUnit.SECONDS);
- Scene myScene;
+ Scene myScene = new Scene();
// read initial scene
Path initialSceneFile = UtilB.pathToDirectoryOfPlaceB().resolve(filenameInitialSceneB);
- de.tudresden.inf.st.ceti.Scene initialScene = Util.readScene(initialSceneFile);
- if (scenario.loadAndChangeScenes) {
- myScene = UtilB.convert(initialScene);
- } else {
- myScene = new Scene();
-
- mqttHandler.newConnection("coordinating/rag-b/command", bytes -> {
- if (new String(bytes).equals("start")) {
- mqttHandler.publish(scenario.topicSceneUpdateB, initialScene.toByteArray());
- }
- });
+ if (initialSceneFile.toFile().exists()) {
+ de.tudresden.inf.st.ceti.Scene initialScene = Util.readScene(initialSceneFile);
+ if (scenario.loadAndChangeScenes) {
+ myScene = UtilB.convert(initialScene);
+ } else {
+ mqttHandler.newConnection("coordinating/rag-b/command", bytes -> {
+ if (new String(bytes).equals("start")) {
+ mqttHandler.publish(scenario.topicSceneUpdateB, initialScene.toByteArray());
+ }
+ });
+ }
}
model.setMyScene(myScene);
@@ -130,9 +127,9 @@ public class SimpleMainB {
UtilB.setRegions(model, Util.parseRegionConfig(regionBFile));
// init robots and reachability
- for (ReachabilityConfig reachabilityConfig : config.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(reachabilityConfig.filename));
+ Path path = UtilB.pathToDirectoryOfPlaceB().resolve(Paths.get(reachabilityFileName));
Reachability reachability = UtilB.readReachability(path);
Robot robot = UtilB.createRobot(reachability.getIdRobot());
@@ -188,7 +185,9 @@ public class SimpleMainB {
LogicalScene logicalSceneFromPlaceA = sceneFromPlaceA.getLogicalScene();
byte[] bytesToSend = _ragconnect__apply__TreeDefaultLogicalSceneToBytesMapping(logicalSceneFromPlaceA);
- if (scenario.loadAndChangeScenes) {
+ if (scenario.loadAndChangeScenes && initialSceneFile.toFile().exists()) {
+ de.tudresden.inf.st.ceti.Scene initialScene = Util.readScene(initialSceneFile);
+
describedWait(1, "send new logical scene" );
mqttHandler.publish(topicUpdateFromPlaceA, bytesToSend);
@@ -246,7 +245,7 @@ public class SimpleMainB {
de.tudresden.inf.st.ceti.Scene scene,
@SuppressWarnings("SameParameterValue" ) String objectName,
Position newPosition) {
- return updateObject(scene, objectName, obj -> {
+ return UtilB.updateObject(scene, objectName, obj -> {
Object.Builder builder = obj.toBuilder();
builder.getPosBuilder()
.setX(newPosition.getX())
@@ -259,30 +258,7 @@ public class SimpleMainB {
static de.tudresden.inf.st.ceti.Scene updateNotBusyOfRobot(
de.tudresden.inf.st.ceti.Scene scene,
String objectName) {
- return updateObject(scene, objectName, obj -> obj.toBuilder().setActive(false).build());
- }
-
- static de.tudresden.inf.st.ceti.Scene updateObject(
- de.tudresden.inf.st.ceti.Scene scene,
- String objectName,
- Function<Object, Object> change) {
- List<Object> objectsList = scene.getObjectsList();
- Object newObj = null;
- int index, objectsListSize;
- for (index = 0, objectsListSize = objectsList.size(); index < objectsListSize; index++) {
- Object obj = objectsList.get(index);
- if (obj.getId().equals(objectName)) {
- newObj = change.apply(obj);
- break;
- }
- }
- if (newObj == null) {
- logger.error("Did not find object {}!", objectName);
- } else {
- scene = scene.toBuilder().setObjects(index, newObj).build();
- logger.info("Update {} in scene to:\n {}", objectName, newObj);
- }
- return scene;
+ return UtilB.updateObject(scene, objectName, obj -> obj.toBuilder().setActive(false).build());
}
diff --git a/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/UtilB.java b/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/UtilB.java
index d58dc9e..0619a5a 100644
--- a/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/UtilB.java
+++ b/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/UtilB.java
@@ -1,6 +1,7 @@
package de.tudresden.inf.st.placeB;
import com.google.protobuf.util.JsonFormat;
+import de.tudresden.inf.st.ceti.Object;
import de.tudresden.inf.st.ceti.Reachability;
import de.tudresden.inf.st.placeB.ast.*;
import de.tudresden.inf.st.ros3rag.common.RegionConfiguration;
@@ -12,7 +13,8 @@ import org.apache.logging.log4j.Logger;
import java.io.IOException;
import java.nio.file.Files;
import java.nio.file.Path;
-import java.util.stream.Collectors;
+import java.util.List;
+import java.util.function.Function;
/**
* Static utility methods used only for place B.
@@ -153,6 +155,29 @@ public class UtilB {
model.setRegionList(result);
}
+ static de.tudresden.inf.st.ceti.Scene updateObject(
+ de.tudresden.inf.st.ceti.Scene scene,
+ String objectName,
+ Function<Object, Object> change) {
+ List<Object> objectsList = scene.getObjectsList();
+ Object newObj = null;
+ int index, objectsListSize;
+ for (index = 0, objectsListSize = objectsList.size(); index < objectsListSize; index++) {
+ Object obj = objectsList.get(index);
+ if (obj.getId().equals(objectName)) {
+ newObj = change.apply(obj);
+ break;
+ }
+ }
+ if (newObj == null) {
+ logger.error("Did not find object {}!", objectName);
+ } else {
+ scene = scene.toBuilder().setObjects(index, newObj).build();
+ logger.info("Update {} in scene to:\n {}", objectName, newObj);
+ }
+ return scene;
+ }
+
@SuppressWarnings("rawtypes")
static class ExposingASTNode extends ASTNode {
public Scene exposed_apply_ConvertScene(de.tudresden.inf.st.ceti.Scene pbScene) throws Exception {
diff --git a/ros3rag.placeB/src/main/resources/config-b-placeworld.yaml b/ros3rag.placeB/src/main/resources/config-b-placeworld.yaml
index c6d99b6..72a4db5 100644
--- a/ros3rag.placeB/src/main/resources/config-b-placeworld.yaml
+++ b/ros3rag.placeB/src/main/resources/config-b-placeworld.yaml
@@ -1,6 +1,11 @@
-mqttHost: "localhost"
-useReachability: true
-reachability:
- - filename: "src/main/resources/reachability-b-arm1-placeworld.json"
- - filename: "src/main/resources/reachability-b-arm2-placeworld.json"
+mqttHost: "192.168.0.122"
+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"
coordinatorMqttTopicPrefix: "coordinating/rag-b"
diff --git a/ros3rag.placeB/src/main/resources/regions-b-placeworld.json b/ros3rag.placeB/src/main/resources/regions-b-placeworld.json
index 19abce0..da798ae 100644
--- a/ros3rag.placeB/src/main/resources/regions-b-placeworld.json
+++ b/ros3rag.placeB/src/main/resources/regions-b-placeworld.json
@@ -4,6 +4,7 @@
{ "name": "B", "positions": ["B1", "B2"] },
{ "name": "C", "positions": ["C1"] },
{ "name": "G", "positions": ["G1"] },
- { "name": "E", "positions": ["E1"] }
+ { "name": "E", "positions": ["E1"] },
+ { "name": "CZ", "positions": ["cz1"] }
]
}
--
GitLab