diff --git a/open-latest-svg-b.sh b/open-latest-svg-b.sh
index 34fd6ee4b295c3f8fb24c8518e85162373263f0d..7f78222d7a9c2c82dd12353122c3691701b545ae 100755
--- a/open-latest-svg-b.sh
+++ b/open-latest-svg-b.sh
@@ -1,2 +1,2 @@
#!/usr/bin/env bash
-ls -c ros3rag.placeB/*.svg | head -n1 | xargs xdg-open
+ls -c ros3rag.placeB/images/*.svg | head -n1 | xargs xdg-open
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 962e31ac5a8a74840a9985b5cd71c9b9026166af..a0ac80bcc753c4681a07f04103de219a586b0996 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
@@ -13,6 +13,7 @@ import java.util.List;
public class Configuration {
public String mqttHost;
public String filenameInitialScene;
+ public String filenameRegions;
public boolean useReachability;
public String coordinatorMqttTopicPrefix;
public List<ReachabilityConfig> reachability;
diff --git a/ros3rag.common/src/main/proto/cgv_connector.proto b/ros3rag.common/src/main/proto/cgv_connector.proto
index b80690640592cccf383f78abf439d1a86b2ab901..0a7e924d8551cd82fb9db4123d4aca65c98bb074 100644
--- a/ros3rag.common/src/main/proto/cgv_connector.proto
+++ b/ros3rag.common/src/main/proto/cgv_connector.proto
@@ -1,5 +1,5 @@
-// cgv_connector.proto
-// this file contains the messages that are exchanged between the cgv framework and the st ROS interface
+// connector.proto
+// this file contains the messages that are exchanged between the ROS connector and other systems
syntax = "proto3";
@@ -10,22 +10,22 @@ message Object {
// Position is object-center related
message Position {
- float x = 1; // in m
- float y = 2; // in m
- float z = 3; // height in m
+ double x = 1; // in m
+ double y = 2; // in m
+ double z = 3; // height in m
}
// 3D description of the object
message Size {
- float length = 1; // in m
- float width = 2; // in m
- float height = 3; // in m
+ double length = 1; // in m
+ double width = 2; // in m
+ double height = 3; // in m
}
message Orientation {
- float x = 1; // normalized quaternion
- float y = 2;
- float z = 3;
- float w = 4;
+ double x = 1; // normalized quaternion
+ double y = 2;
+ double z = 3;
+ double w = 4;
}
message Color {
float r = 1; // 0..1
@@ -38,6 +38,9 @@ message Object {
BIN = 2;
ARM = 3;
DROP_OFF_LOCATION = 4;
+ HUMAN = 5;
+ ROBOT = 6;
+ COLLABORATION_ZONE = 7;
}
string id = 1;
@@ -46,9 +49,10 @@ message Object {
Size size = 4;
Orientation orientation = 5;
Color color = 6;
+ bool active = 7;
}
-// the scene is stored within the ROS side and sent to the CGV framework
+// the scene is stored within the ROS side and sent to clients
message Scene {
repeated Object objects = 1;
}
@@ -59,9 +63,15 @@ message Selection {
string id = 1; // the id corresponds to an id of an Object in a Scene
}
-// vvv from rs vvv.
-// Merged message to contain both pick and place in one
-message MergedSelection {
+message Command {
+ oneof msg {
+ PickAndPlace pickAndPlace = 1;
+ ConfigChange configChange = 2;
+ Evacuate evacuate = 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.
@@ -74,6 +84,7 @@ message ConfigChange {
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
}
// Reachability of objects, as reported by MoveIt
diff --git a/ros3rag.common/src/main/resources/jastadd/types.connect b/ros3rag.common/src/main/resources/jastadd/types.connect
index 84730eb21d622f26726f658c247e9d76a860cc11..dd61bb52227a984bdb2c512505b56b33727777ea 100644
--- a/ros3rag.common/src/main/resources/jastadd/types.connect
+++ b/ros3rag.common/src/main/resources/jastadd/types.connect
@@ -19,6 +19,13 @@ ConvertScene maps de.tudresden.inf.st.ceti.Scene pbScene to Scene {:
result.addDropOffLocation(dropOffLocation);
obj = dropOffLocation;
break;
+ case COLLABORATION_ZONE:
+ // COLLABORATION_ZONE == CollaborationZone
+ CollaborationZone cz = new CollaborationZone();
+ result.addDropOffLocation(cz);
+ obj = cz;
+ break;
+ // TODO maybe add ARM here as well?
default:
// ignore object, continue for loop
continue;
diff --git a/ros3rag.common/src/main/resources/jastadd/types.jadd b/ros3rag.common/src/main/resources/jastadd/types.jadd
index 55c36d43513dee2b85dc319034fc213b2e1bf7e7..f31b6ab7f9579f46203e7b1c00728a15c3cea427 100644
--- a/ros3rag.common/src/main/resources/jastadd/types.jadd
+++ b/ros3rag.common/src/main/resources/jastadd/types.jadd
@@ -33,14 +33,10 @@ aspect Resolving {
return null;
}
-// syn LocationMappingPair LocationMapping.findMappingFor(LogicalDropOffLocation location) {
-// for (LocationMappingPair pair : getLocationMappingPairList()) {
-// if (pair.getLogicalLocation().equals(location)) {
-// return pair;
-// }
-// }
-// return null;
-// }
+ syn DropOffLocation LogicalMovableObject.myLocation() {
+ // classical reference attribute (was relation, but not allowed with RagConnect)
+ return containingScene().resolveObjectOfInterest(getNameOfMyLocation()).asDropOffLocation();
+ }
}
aspect Computation {
@@ -111,7 +107,7 @@ aspect Computation {
if (movableObject.isLocatedAt(location)) {
LogicalMovableObject logicalObject = objects.get(movableObject);
logicalRegion.addContainedObject(logicalObject);
- logicalObject.setMyLocation(location);
+ logicalObject.setNameOfMyLocation(location.getName());
}
}
}
@@ -165,8 +161,8 @@ aspect Navigation {
inh Scene DropOffLocation.containingScene();
inh Scene MovableObject.containingScene();
- eq Scene.getDropOffLocation().containingScene() = this;
- eq Scene.getMovableObject().containingScene() = this;
+ inh Scene LogicalMovableObject.containingScene();
+ eq Scene.getChild().containingScene() = this;
syn boolean LogicalMovableObject.hasLocatedAt() = !getLocatedAtList().isEmpty();
@@ -293,15 +289,15 @@ aspect ConvenienceMethods {
object.setSize(size);
}
- public static Position Position.of(float x, float y, float z) {
+ public static Position Position.of(double x, double y, double z) {
return new Position(x, y, z);
}
- public static Orientation Orientation.of(float x, float y, float z, float w) {
+ public static Orientation Orientation.of(double x, double y, double z, double w) {
return new Orientation(x, y, z, w);
}
- public static Size Size.of(float length, float width, float height) {
+ public static Size Size.of(double length, double width, double height) {
return new Size(length, width, height);
}
}
diff --git a/ros3rag.common/src/main/resources/jastadd/types.relast b/ros3rag.common/src/main/resources/jastadd/types.relast
index 783bf00d21f8ec7a6ef01e25437dee6efc830f93..0be348773e8fb7fd372d3a648267c72877d1138c 100644
--- a/ros3rag.common/src/main/resources/jastadd/types.relast
+++ b/ros3rag.common/src/main/resources/jastadd/types.relast
@@ -1,6 +1,6 @@
-Position ::= <X:float> <Y:float> <Z:float> ;
-Size ::= <Length:float> <Width:float> <Height:float> ;
-Orientation ::= <X:float> <Y:float> <Z:float> <W:float> ;
+Position ::= <X:double> <Y:double> <Z:double> ;
+Size ::= <Length:double> <Width:double> <Height:double> ;
+Orientation ::= <X:double> <Y:double> <Z:double> <W:double> ;
// Regions cannot be contained in scene, but must be retrieved via attribute
Scene ::= DropOffLocation* MovableObject* /LogicalScene/ ;
@@ -17,11 +17,11 @@ CanReachObjectOfInterest ::= <ObjectName:String> ;
LogicalScene ::= LogicalRegion* LogicalMovableObject* ;
LogicalObjectOfInterest ::= <Name:String> ;
LogicalRegion : LogicalObjectOfInterest ;
-LogicalMovableObject : LogicalObjectOfInterest ;
+LogicalMovableObject : LogicalObjectOfInterest ::= <NameOfMyLocation> ;
rel LogicalRegion.ContainedObject* <-> LogicalMovableObject.LocatedAt* ;
// TODO could lead to problems when including this information and sending it
-rel LogicalMovableObject.MyLocation? -> DropOffLocation ;
+//rel LogicalMovableObject.MyLocation? -> DropOffLocation ;
Region ::= <Name:String> ;
rel Region.Location* <-> DropOffLocation.ContainedInRegion* ;
diff --git a/ros3rag.common/src/main/resources/tasks.gradle b/ros3rag.common/src/main/resources/tasks.gradle
index 8af9da6e89a4be3a0dea360a6d5acecf1807e064..4b33d29322bfb1f43c620fa3d236a5f45e63cea5 100644
--- a/ros3rag.common/src/main/resources/tasks.gradle
+++ b/ros3rag.common/src/main/resources/tasks.gradle
@@ -1,6 +1,6 @@
dependencies {
- jastadd2 "org.jastadd:jastadd2:2.3.5-dresden-5"
+ jastadd2 "org.jastadd:jastadd2:2.3.5-dresden-6"
// jastadd2 fileTree(include: ['jastadd2.jar'], dir: '../libs')
api group: 'org.fusesource.mqtt-client', name: 'mqtt-client', version: '1.15'
}
diff --git a/ros3rag.placeA/src/main/resources/config-scene-a-mini.json b/ros3rag.placeA/src/main/resources/config-scene-a-mini.json
new file mode 100644
index 0000000000000000000000000000000000000000..b1979a3913b9c3762c531cf70a7d9f1bd9fb5175
--- /dev/null
+++ b/ros3rag.placeA/src/main/resources/config-scene-a-mini.json
@@ -0,0 +1,5 @@
+{ "objects": [
+ { "id": "P1","type": "DROP_OFF_LOCATION","pos": { "x": 1,"y": 2,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "b": 1 } },
+ { "id": "P2","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": "O1","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 } }
+] }
diff --git a/ros3rag.placeA/src/main/resources/regions-a.json b/ros3rag.placeA/src/main/resources/regions-a-2022.json
similarity index 100%
rename from ros3rag.placeA/src/main/resources/regions-a.json
rename to ros3rag.placeA/src/main/resources/regions-a-2022.json
diff --git a/ros3rag.placeA/src/main/resources/regions-a-mini.json b/ros3rag.placeA/src/main/resources/regions-a-mini.json
new file mode 100644
index 0000000000000000000000000000000000000000..e699d7cb34992104df5b52eeb544d6cecce5fcdc
--- /dev/null
+++ b/ros3rag.placeA/src/main/resources/regions-a-mini.json
@@ -0,0 +1,6 @@
+{
+ "regions": [
+ { "name": "Reg1", "positions": ["P1"] },
+ { "name": "Reg2", "positions": ["P2"] }
+ ]
+}
diff --git a/ros3rag.placeB/.gitignore b/ros3rag.placeB/.gitignore
index e6a248d26d9db140d397c0b81d57239b09f09777..3447a4e17f56acc57a97d96cfec3bd43d90a05d9 100644
--- a/ros3rag.placeB/.gitignore
+++ b/ros3rag.placeB/.gitignore
@@ -3,5 +3,4 @@ src/gen-res/
src/gen/
out/
*.class
-*.png
-*.svg
+images/
diff --git a/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.jrag b/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.jrag
index d20b32b2ff383f978ecb9ba68de19f66c181639f..93adc36cd2233a6b90140e2817c1cb8d17e6df86 100644
--- a/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.jrag
+++ b/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.jrag
@@ -8,8 +8,8 @@ aspect RobotReachabilityToBFS {
Map<ObjectOfInterest, Vertex> mapping = new HashMap<>();
// TODO insert vertices for objects, if they are contained in a location. or remove objects altogether?
for (DropOffLocation loc : getMyScene().getDropOffLocationList()) {
- Vertex vertex = new Vertex();
- vertex.setObjectOfInterest(loc);
+ VertexWithObjectOfInterest vertex = new VertexWithObjectOfInterest();
+ vertex.setNameOfObjectOfInterest(loc.getName());
result.addVertex(vertex);
mapping.put(loc, vertex);
}
@@ -20,6 +20,9 @@ aspect RobotReachabilityToBFS {
// mapping.put(obj, vertex);
// }
for (Robot robot : getRobotList()) {
+ if (robot.getBusy()) {
+ continue;
+ }
List<DropOffLocation> reachableLocations = robot.reachableObjects().stream()
.filter(ObjectOfInterest::isDropOffLocation)
.map(ObjectOfInterest::asDropOffLocation)
@@ -31,8 +34,8 @@ aspect RobotReachabilityToBFS {
for (DropOffLocation other : reachableLocations) {
if (loc == other) { continue; }
if (loc.getObjectLocatedHere() != null && other.getObjectLocatedHere() != null) { continue; }
- Edge edge = new Edge();
- edge.setRobot(robot);
+ EdgeWithRobot edge = new EdgeWithRobot();
+ edge.setNameOfRobot(robot.getName());
edge.setBidirectional(loc.getObjectLocatedHere() == null);
edge.setFrom(mapping.get(loc));
edge.setTo(mapping.get(other));
@@ -51,7 +54,7 @@ aspect RobotReachabilityToBFS {
return Optional.empty();
}
for (Vertex v : toReachabilityGraph().getVertexList()) {
- if (v.getObjectOfInterest().getName().equals(name)) {
+ if (v.isVertexWithObjectOfInterest() && v.asVertexWithObjectOfInterest().getNameOfObjectOfInterest().equals(name)) {
return Optional.of(v);
}
}
@@ -60,10 +63,28 @@ aspect RobotReachabilityToBFS {
}
aspect Printint {
- public String Vertex.toString() {
- return "V(" + getObjectOfInterest().getName() + ")";
+ public String VertexWithObjectOfInterest.toString() {
+ return "V(" + getNameOfObjectOfInterest() + ")";
}
- public String Edge.toString() {
- return "E[" + getRobot().getName() + "]";
+ public String EdgeWithRobot.toString() {
+ return "E[" + getNameOfRobot() + "]";
}
}
+
+aspect Navigation {
+ // --- isVertexWithObjectOfInterest ---
+ syn boolean Vertex.isVertexWithObjectOfInterest() = false;
+ eq VertexWithObjectOfInterest.isVertexWithObjectOfInterest() = true;
+
+ // --- asVertexWithObjectOfInterest ---
+ syn VertexWithObjectOfInterest Vertex.asVertexWithObjectOfInterest() = null;
+ eq VertexWithObjectOfInterest.asVertexWithObjectOfInterest() = this;
+
+ // --- isEdgeWithRobot ---
+ syn boolean Edge.isEdgeWithRobot() = false;
+ eq EdgeWithRobot.isEdgeWithRobot() = true;
+
+ // --- asEdgeWithRobot ---
+ syn EdgeWithRobot Edge.asEdgeWithRobot() = null;
+ eq EdgeWithRobot.asEdgeWithRobot() = this;
+}
diff --git a/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.relast b/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.relast
index a73376e73e68958585834372a2f12c4282b943e4..96a4b0cdda9cfda9d2e79037853f6e537a510126 100644
--- a/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.relast
+++ b/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.relast
@@ -1,2 +1,4 @@
-rel Vertex.ObjectOfInterest -> ObjectOfInterest ;
-rel Edge.Robot -> Robot ;
+//rel Vertex.ObjectOfInterest -> ObjectOfInterest ;
+VertexWithObjectOfInterest : Vertex ::= <NameOfObjectOfInterest> ;
+//rel Edge.Robot -> Robot ;
+EdgeWithRobot : Edge ::= <NameOfRobot> ;
diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.connect b/ros3rag.placeB/src/main/jastadd/WorldModelB.connect
index 09270e201371e33e79de133fbdca1c1afbfad27e..6529e04857416ab2731a78e83514a894da042905 100644
--- a/ros3rag.placeB/src/main/jastadd/WorldModelB.connect
+++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.connect
@@ -18,13 +18,18 @@ PrintOperation maps Operation op to byte[] {:
:}
ConfigChangeCommandCheckForOwnedCollaborationZone maps byte[] bytes to String {:
- de.tudresden.inf.st.ceti.ConfigChange cc = null;
+ de.tudresden.inf.st.ceti.Command command = null;
try {
- cc = de.tudresden.inf.st.ceti.ConfigChange.parseFrom(bytes);
+ command = de.tudresden.inf.st.ceti.Command.parseFrom(bytes);
} catch (com.google.protobuf.InvalidProtocolBufferException e) {
reject();
}
- System.out.println(cc);
+ // if the command is not a ConfigChange command, do not change anything
+ if (!command.hasConfigChange()) {
+ reject();
+ }
+ System.out.println(command);
+ de.tudresden.inf.st.ceti.ConfigChange cc = command.getConfigChange();
Scene scene = worldModelB().getMyScene();
ObjectOfInterest obj = scene.resolveObjectOfInterest(cc.getIdCollaborationZone());
DropOffLocation loc = obj.asDropOffLocation();
@@ -45,19 +50,21 @@ ConfigChangeCommandCheckForOwnedCollaborationZone maps byte[] bytes to String {:
:}
ConfigChangeCommandCheckForBusy maps byte[] bytes to boolean {:
- de.tudresden.inf.st.ceti.MergedSelection ms = null;
+ de.tudresden.inf.st.ceti.Command command = null;
try {
- ms = de.tudresden.inf.st.ceti.MergedSelection.parseFrom(bytes);
+ command = de.tudresden.inf.st.ceti.Command.parseFrom(bytes);
} catch (com.google.protobuf.InvalidProtocolBufferException e) {
reject();
}
- System.out.println(ms);
- Robot thisRobot = (Robot) this;
- if (thisRobot.getName().equals(ms.getIdRobot())) {
- return true;
+ System.out.println(command);
+ // if the command is not a PickAndPlace command, do not change anything
+ if (!command.hasPickAndPlace()) {
+ reject();
}
+ Robot thisRobot = (Robot) this;
// if the command is not about this robot, do not change anything
- reject();
- // need return to make compiler happy
- return false;
+ if (!thisRobot.getName().equals(command.getPickAndPlace().getIdRobot())) {
+ reject();
+ }
+ return true;
:}
diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd
index a64332153b7acb7fc81a3429b3b2d687a11b7cda..6a66bfce152081bc38db3f57d9e3d3919d0ff5b8 100644
--- a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd
+++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd
@@ -129,7 +129,17 @@ aspect Computation {
syn List<Operation> Difference.computeOperations();
eq DifferenceObjectAtWrongPlace.computeOperations() {
// need to use position of object and pick one from new region
- DropOffLocation previousLocation = getObject().getMyLocation();
+
+ DropOffLocation previousLocation = getObject().myLocation();
+ if (previousLocation == null) {
+ return error("No previous location");
+ }
+ // (workaround) check if WorldModelB is not null ... could happen if scene has no parent
+ try {
+ previousLocation.correspondingVertex();
+ } catch (NullPointerException e) {
+ return error("Could not resolve WorldModelB");
+ }
return previousLocation.correspondingVertex().map(previousVertex -> {
Region region = getNewRegion().realRegion();
if (region == null) {
@@ -145,30 +155,30 @@ aspect Computation {
if (shortestPath == null || shortestPath.isEmpty()) {
return error("No sequence of operations to move " + getObject().getName() + (hasPreviousRegion() ? " from " + getPreviousRegion().getName() : "") + " to " + getNewRegion().getName());
}
- // TODO insert config-change and evacuate commands
List<Operation> result = new ArrayList<>();
Vertex transit = previousVertex;
for (Edge edge : shortestPath) {
Vertex target = edge.getFrom().equals(transit) ? edge.getTo() : edge.getFrom();
- DropOffLocation targetLocation = target.getObjectOfInterest().asDropOffLocation();
- Robot executingRobot = edge.getRobot();
- 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!
- */
-
- // order is important here, first add ConfigChange, then Evacuate
- CollaborationZone cz = targetLocation.asCollaborationZone();
- if (!cz.hasOwner() || (cz.hasOwner() && !cz.getOwner().equals(executingRobot))) {
- result.add(ConfigChange.of(executingRobot, cz));
- }
- if (cz.hasOccupient() && !cz.getOccupient().equals(executingRobot)) {
- result.add(Evacuate.of(cz.getOccupient()));
+ DropOffLocation targetLocation = worldModelB().getMyScene().resolveObjectOfInterest(target.asVertexWithObjectOfInterest().getNameOfObjectOfInterest()).asDropOffLocation();
+// Robot executingRobot = worldModelB().findRobot(edge.asEdgeWithRobot().getNameOfRobot());
+ worldModelB().findRobot(edge.asEdgeWithRobot().getNameOfRobot()).ifPresentOrElse(executingRobot -> {
+ 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();
+ // order is important here, first add Evacuate, then ConfigChange
+ if (cz.hasOccupient() && !cz.getOccupient().equals(executingRobot)) {
+ result.add(Evacuate.of(cz.getOccupient(), cz));
+ }
+ if (!cz.hasOwner() || (cz.hasOwner() && !cz.getOwner().equals(executingRobot))) {
+ result.add(ConfigChange.of(executingRobot, cz));
+ }
}
- }
- result.add(PickAndPlace.of(executingRobot, getObject(), targetLocation));
+ result.add(PickAndPlace.of(executingRobot, getObject(), targetLocation));
+ }, () -> result.add(new ErrorOperation("Could not find robot with name " + edge.asEdgeWithRobot().getNameOfRobot())));
transit = target;
}
return result;
@@ -206,9 +216,10 @@ aspect Computation {
return result;
}
- public static Evacuate Evacuate.of(Robot robot) {
+ public static Evacuate Evacuate.of(Robot robot, CollaborationZone cz) {
Evacuate result = new Evacuate();
result.setRobotToExecute(robot);
+ result.setCollaborationZone(cz);
return result;
}
@@ -301,17 +312,24 @@ aspect AttributeMappings {
eq Place.toProtobufByteArray() {
throw new RuntimeException("Separate Place operation not supported yet!");
}
- eq PickAndPlace.toProtobufByteArray() = de.tudresden.inf.st.ceti.MergedSelection.newBuilder()
- .setIdRobot(getRobotToExecute().getName())
- .setIdPick(getObjectToPick().getName())
- .setIdPlace(getTargetLocation().getName())
+ eq PickAndPlace.toProtobufByteArray() = de.tudresden.inf.st.ceti.Command.newBuilder()
+ .setPickAndPlace(de.tudresden.inf.st.ceti.PickAndPlace.newBuilder()
+ .setIdRobot(getRobotToExecute().getName())
+ .setIdPick(getObjectToPick().getName())
+ .setIdPlace(getTargetLocation().getName())
+ .build())
.build().toByteArray();
- eq ConfigChange.toProtobufByteArray() = de.tudresden.inf.st.ceti.ConfigChange.newBuilder()
- .setIdRobotNewOwner(getRobotToExecute().getName())
- .setIdCollaborationZone(getCollaborationZone().getName())
+ eq ConfigChange.toProtobufByteArray() = de.tudresden.inf.st.ceti.Command.newBuilder()
+ .setConfigChange(de.tudresden.inf.st.ceti.ConfigChange.newBuilder()
+ .setIdRobotNewOwner(getRobotToExecute().getName())
+ .setIdCollaborationZone(getCollaborationZone().getName())
+ .build())
.build().toByteArray();
- eq Evacuate.toProtobufByteArray() = de.tudresden.inf.st.ceti.Evacuate.newBuilder()
- .setIdRobot(getRobotToExecute().getName())
+ eq Evacuate.toProtobufByteArray() = de.tudresden.inf.st.ceti.Command.newBuilder()
+ .setEvacuate(de.tudresden.inf.st.ceti.Evacuate.newBuilder()
+ .setIdRobot(getRobotToExecute().getName())
+ .setIdCollaborationZone(getCollaborationZone().getName())
+ .build())
.build().toByteArray();
}
@@ -370,7 +388,7 @@ aspect Printing {
return "+ConfigChange: " + getRobotToExecute().prettyPrint() + " owns " + getCollaborationZone().prettyPrint() + "+";
}
eq Evacuate.prettyPrint() {
- return "+Evacuate: " + getRobotToExecute().prettyPrint() + "+";
+ return "+Evacuate: " + getRobotToExecute().prettyPrint() + " from " + getCollaborationZone().prettyPrint() + "+";
}
}
@@ -380,8 +398,8 @@ aspect DumpAst {
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(now + ".png"));
- builder.dumpAsSVG(java.nio.file.Paths.get(now + ".svg"));
+ //builder.dumpAsPNG(java.nio.file.Paths.get("images/" + now + ".png"));
+ builder.dumpAsSVG(java.nio.file.Paths.get("images/" + now + ".svg"));
} catch (java.io.IOException e) {
e.printStackTrace();
}
@@ -409,6 +427,6 @@ aspect Resolving {
return null;
}
- // do not resolve real regions and real locations (send by site-A)
- refine RefResolverStubs eq LogicalMovableObject.resolveMyLocationByToken(String name) = null;
+// // do not resolve real regions and real locations (send by site-A)
+// refine RefResolverStubs eq LogicalMovableObject.resolveMyLocationByToken(String name) = null;
}
diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.relast b/ros3rag.placeB/src/main/jastadd/WorldModelB.relast
index 7e45d74c5cb4ff0850e04c26d9caf46caf22bbac..919d3724f5cc1e4e6b780ce023810b3f2e0f9fc0 100644
--- a/ros3rag.placeB/src/main/jastadd/WorldModelB.relast
+++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.relast
@@ -31,3 +31,4 @@ ConfigChange : Operation ;
rel ConfigChange.CollaborationZone -> CollaborationZone ;
Evacuate : Operation ;
+rel Evacuate.CollaborationZone -> CollaborationZone ;
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 c9e5e34d3e3b5b677a258e94cb232bef6b43a616..c65771d900c2079ef3dbb4adba8b2cd2a35b7618 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
@@ -1,5 +1,6 @@
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;
@@ -27,38 +28,40 @@ import java.util.stream.Collectors;
public class SimpleMainB {
private static final Logger logger = LogManager.getLogger(SimpleMainB.class);
boolean exitAutomatically = true;
+ boolean useMini = true;
public static void main(String[] args) throws Exception {
System.out.println("Running SimpleMainB");
new SimpleMainB().run(args);
}
- @SuppressWarnings({"unused", "CommentedOutCode"})
+ @SuppressWarnings({"unused"})
private void run(String[] args) throws Exception {
-// testBuildModelB();
-// testReceivingModelB();
-// testReachability();
-// testWeirdBehaviour();
-// testReachability();
-// testBFS();
-// testRobotReachabilityBFS();
readModelAndReceiveFromA();
}
private void readModelAndReceiveFromA() throws Exception {
+ final String suffix = useMini ? "mini" : "2022";
Configuration config = new Configuration();
config.mqttHost = "localhost";
+ config.filenameInitialScene = "src/main/resources/config-scene-b-" + suffix + ".json";
+ config.filenameRegions = "src/main/resources/regions-b-" + suffix + ".json";
ReachabilityConfig reachabilityR1 = new ReachabilityConfig();
reachabilityR1.idRobot = "R1";
- reachabilityR1.filename = "src/main/resources/reachability-b-r1-2022.json";
+ reachabilityR1.filename = "src/main/resources/reachability-b-r1-" + suffix + ".json";
ReachabilityConfig reachabilityR2 = new ReachabilityConfig();
reachabilityR2.idRobot = "R2";
- reachabilityR2.filename = "src/main/resources/reachability-b-r2-2022.json";
+ reachabilityR2.filename = "src/main/resources/reachability-b-r2-" + suffix + ".json";
config.reachability = Arrays.asList(reachabilityR1, reachabilityR2);
+ Configuration configA = new Configuration();
+ configA.filenameInitialScene = "src/main/resources/config-scene-a-" + suffix + ".json";
+ configA.filenameRegions = "src/main/resources/regions-a-" + suffix + ".json";
+
+ final String topicSceneUpdateB = "place-b/scene/update";
final String topicUpdateFromPlaceA = "update/logical/fromA";
- final String topicCommand = "command";
+ final String topicCommand = "place-b/command";
final String topicExit = "place-b/exit";
final String topicModel = "place-b/model";
final String topicModelStatus = "place-b/status";
@@ -72,27 +75,13 @@ public class SimpleMainB {
model.ragconnectSetupMqttWaitUntilReady(2, TimeUnit.SECONDS);
// read initial scene
- Path initialSceneFile = UtilB.pathToDirectoryOfPlaceB().resolve("src/main/resources/config-scene-b-2022.json");
+ Path initialSceneFile = UtilB.pathToDirectoryOfPlaceB().resolve(config.filenameInitialScene);
de.tudresden.inf.st.ceti.Scene scene = Util.readScene(initialSceneFile);
Scene myScene = UtilB.convert(scene);
- // change locations P-E and P-F to collaboration zones
- for (String collaborationZoneName : new String[]{ "P-E", "P-F" }) {
- DropOffLocation loc = myScene.resolveObjectOfInterest(collaborationZoneName).asDropOffLocation();
- CollaborationZone cz = new CollaborationZone();
- cz.setName(collaborationZoneName);
- cz.setPosition(loc.getPosition());
- cz.setOrientation(loc.getOrientation());
- cz.setSize(loc.getSize());
- int index = myScene.getDropOffLocationList().getIndexOfChild(loc);
- if (index == -1) {
- throw new RuntimeException("Could not find location " + collaborationZoneName + " in list");
- }
- myScene.setDropOffLocation(cz, index);
- }
model.setMyScene(myScene);
// read and set regions
- File regionBFile = UtilB.pathToDirectoryOfPlaceB().resolve("src/main/resources/regions-b.json").toFile();
+ File regionBFile = UtilB.pathToDirectoryOfPlaceB().resolve(config.filenameRegions).toFile();
UtilB.setRegions(model, Util.parseRegionConfig(regionBFile));
// init robots (copied from MainB)
@@ -121,6 +110,7 @@ public class SimpleMainB {
Runtime.getRuntime().addShutdownHook(new Thread(close));
model.connectOtherScene(Util.mqttUri(topicUpdateFromPlaceA, config), 0);
+ model.connectMyScene(Util.mqttUri(topicSceneUpdateB, config));
for (Robot robot : model.getRobotList()) {
robot.connectBusy(Util.mqttUri(topicCommand, config));
robot.connectDummyOwnedCollaborationZone(Util.mqttUri(topicCommand, config));
@@ -148,8 +138,8 @@ public class SimpleMainB {
});
// read scene from A, serialize it and send it via mqtt
- Path sceneAFile = Util.pathToModuleDirectory("ros3rag.placeA").resolve("src/main/resources/config-scene-a-2022.json");
- File regionAFile = Util.pathToModuleDirectory("ros3rag.placeA").resolve("src/main/resources/regions-a.json").toFile();
+ Path sceneAFile = Util.pathToModuleDirectory("ros3rag.placeA").resolve(configA.filenameInitialScene);
+ File regionAFile = Util.pathToModuleDirectory("ros3rag.placeA").resolve(configA.filenameRegions).toFile();
de.tudresden.inf.st.ceti.Scene scenePlaceA = Util.readScene(sceneAFile);
Scene sceneFromPlaceA = UtilB.convert(scenePlaceA);
// need to "wrap" the scene in a world model to access regions. will use one from site-B here for convenience
@@ -167,8 +157,41 @@ public class SimpleMainB {
TimeUnit.SECONDS.sleep(2);
mqttHandler.publish(topicModel, "detailed".getBytes(StandardCharsets.UTF_8));
+ logger.info("Wait another 2 sec, then send updated sceneB");
+ TimeUnit.SECONDS.sleep(2);
+
+ // set object O1 to position of P-E in sceneB and publish it
+ Position posOfPE = myScene.resolveObjectOfInterest("P-E").getPosition();
+
+ 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("O1")) {
+ Object.Builder builder = obj.toBuilder();
+ builder.getPosBuilder()
+ .setX(posOfPE.getX())
+ .setY(posOfPE.getY())
+ .setZ(posOfPE.getZ());
+ newObj = builder.build();
+ break;
+ }
+ }
+ if (newObj == null) {
+ logger.error("Did not find object O1!");
+ } else {
+ scene = scene.toBuilder().setObjects(index, newObj).build();
+ logger.info("New scene: {}", scene);
+ mqttHandler.publish(topicSceneUpdateB, scene.toByteArray());
+ }
+
+ logger.info("Wait another 2 sec, then print model status");
+ TimeUnit.SECONDS.sleep(2);
+ mqttHandler.publish(topicModel, "detailed".getBytes(StandardCharsets.UTF_8));
+
if (exitAutomatically) {
- TimeUnit.SECONDS.sleep(3);
+ TimeUnit.SECONDS.sleep(5);
exitCondition.countDown();
}
@@ -296,10 +319,10 @@ public class SimpleMainB {
}
}
- private void check(float x, float y, float z, Position position, String robotName, String locationName) {
- float dx = x - position.getX();
- float dy = y - position.getY();
- float dz = z - position.getZ();
+ private void check(double x, double y, double z, Position position, String robotName, String locationName) {
+ double dx = x - position.getX();
+ double dy = y - position.getY();
+ double dz = z - position.getZ();
if (Math.sqrt(dx*dx + dy*dy) < 0.05) {
System.out.println(robotName + " too close to " + locationName);
diff --git a/ros3rag.placeB/src/main/resources/config-scene-b-mini.json b/ros3rag.placeB/src/main/resources/config-scene-b-mini.json
new file mode 100644
index 0000000000000000000000000000000000000000..56a911f66dc9f0d6bcf09406ae197296f4549d62
--- /dev/null
+++ b/ros3rag.placeB/src/main/resources/config-scene-b-mini.json
@@ -0,0 +1,10 @@
+{ "objects": [
+ { "id": "P1","type": "DROP_OFF_LOCATION","pos": { "x": 1,"y": 2,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "b": 1 } },
+ { "id": "P2.1","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": "P2.2","type": "DROP_OFF_LOCATION","pos": { "x": 8,"y": 4,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "r": 1, "g": 1 } },
+ { "id": "P-E","type": "COLLABORATION_ZONE","pos": { "x": 5,"y": 4,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "r": 1, "b": 1 } },
+ { "id": "O1","type": "BOX","pos": { "x": 1,"y": 2,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "r": 1 } },
+ { "id": "O2","type": "BOX","pos": { "x": 8,"y": 3, "z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "w": 1 },"color": { "r": 1 } },
+ { "id": "R1","type": "ARM","pos": { "x": 1.5, "y": 3, "z": 0.75 },"size": { },"orientation": { "w": 1 },"color": { "r": 1.00,"g": 1.00,"b": 1.00 } },
+ { "id": "R2","type": "ARM","pos": { "x": 8, "y": 3, "z": 0.75 },"size": { },"orientation": { "w": 1 },"color": { "r": 1.00,"g": 1.00,"b": 1.00 } }
+] }
diff --git a/ros3rag.placeB/src/main/resources/reachability-b-r1-mini.json b/ros3rag.placeB/src/main/resources/reachability-b-r1-mini.json
new file mode 100644
index 0000000000000000000000000000000000000000..d63238f3e54a5d041efd2e89dcaad640c7a0d665
--- /dev/null
+++ b/ros3rag.placeB/src/main/resources/reachability-b-r1-mini.json
@@ -0,0 +1,9 @@
+{
+ "idRobot": "R1",
+ "objects": [
+ { "idObject": "P1", "reachable": true },
+ { "idObject": "P2.1", "reachable": false },
+ { "idObject": "P2.2", "reachable": false },
+ { "idObject": "P-E", "reachable": true }
+ ]
+}
diff --git a/ros3rag.placeB/src/main/resources/reachability-b-r2-mini.json b/ros3rag.placeB/src/main/resources/reachability-b-r2-mini.json
new file mode 100644
index 0000000000000000000000000000000000000000..7d38b49803db68ec4c0d16ec8d0e54c39165c506
--- /dev/null
+++ b/ros3rag.placeB/src/main/resources/reachability-b-r2-mini.json
@@ -0,0 +1,9 @@
+{
+ "idRobot": "R1",
+ "objects": [
+ { "idObject": "P1", "reachable": false },
+ { "idObject": "P2.1", "reachable": true },
+ { "idObject": "P2.2", "reachable": true },
+ { "idObject": "P-E", "reachable": true }
+ ]
+}
diff --git a/ros3rag.placeB/src/main/resources/regions-b.json b/ros3rag.placeB/src/main/resources/regions-b-2022.json
similarity index 100%
rename from ros3rag.placeB/src/main/resources/regions-b.json
rename to ros3rag.placeB/src/main/resources/regions-b-2022.json
diff --git a/ros3rag.placeB/src/main/resources/regions-b-mini.json b/ros3rag.placeB/src/main/resources/regions-b-mini.json
new file mode 100644
index 0000000000000000000000000000000000000000..97e1753578abe45ed38cad590427595f5154cbca
--- /dev/null
+++ b/ros3rag.placeB/src/main/resources/regions-b-mini.json
@@ -0,0 +1,7 @@
+{
+ "regions": [
+ { "name": "Reg1", "positions": ["P1"] },
+ { "name": "Reg2", "positions": ["P2.1", "P2.2"] },
+ { "name": "Reg-E", "positions": ["P-E"] }
+ ]
+}