From 6246d60bcc90deb416d5d36058eeeae83acc5005 Mon Sep 17 00:00:00 2001
From: rschoene <rene.schoene@tu-dresden.de>
Date: Wed, 18 May 2022 17:50:57 +0200
Subject: [PATCH] WIP: updated protobuf, working on correct robot position
 handling

---
 .../src/main/proto/cgv_connector.proto        | 16 +++-
 .../src/main/resources/jastadd/types.connect  |  2 +-
 .../src/main/resources/jastadd/types.relast   |  2 +-
 .../src/main/jastadd/WorldModelB.connect      |  2 +
 .../src/main/jastadd/WorldModelB.jadd         | 76 ++++++++++++++-----
 .../src/main/jastadd/WorldModelB.relast       |  2 +-
 .../de/tudresden/inf/st/placeB/MainB.java     | 20 +++--
 .../tudresden/inf/st/placeB/SimpleMainB.java  |  2 +-
 .../config-scene-b-placeworld-manual.json     |  6 +-
 9 files changed, 96 insertions(+), 32 deletions(-)

diff --git a/ros3rag.common/src/main/proto/cgv_connector.proto b/ros3rag.common/src/main/proto/cgv_connector.proto
index 0a7e924..6231318 100644
--- a/ros3rag.common/src/main/proto/cgv_connector.proto
+++ b/ros3rag.common/src/main/proto/cgv_connector.proto
@@ -42,6 +42,19 @@ message Object {
         ROBOT = 6;
         COLLABORATION_ZONE = 7;
     }
+    enum State {
+        STATE_UNKNOWN = 0;
+
+        // robot states
+        STATE_IDLE = 101;
+        STATE_PICKING = 102;
+        STATE_PLACING = 103;
+        STATE_MOVING = 104;
+
+        // object
+        STATE_STATIONARY = 200;
+        STATE_PICKED = 201;
+    }
 
     string id = 1;
     Type type = 2;
@@ -49,7 +62,8 @@ message Object {
     Size size = 4;
     Orientation orientation = 5;
     Color color = 6;
-    bool active = 7;
+    State state = 7;
+    string owner = 8;
 }
 
 // the scene is stored within the ROS side and sent to clients
diff --git a/ros3rag.common/src/main/resources/jastadd/types.connect b/ros3rag.common/src/main/resources/jastadd/types.connect
index 911770f..1d21d81 100644
--- a/ros3rag.common/src/main/resources/jastadd/types.connect
+++ b/ros3rag.common/src/main/resources/jastadd/types.connect
@@ -28,7 +28,7 @@ ConvertScene maps de.tudresden.inf.st.ceti.Scene pbScene to Scene {:
       case ARM:
         // ARM == RobotObject
         RobotObject ro = new RobotObject();
-        ro.setActive(pbObject.getActive());
+        ro.setState(pbObject.getState());
         result.addRobotObject(ro);
         obj = ro;
         break;
diff --git a/ros3rag.common/src/main/resources/jastadd/types.relast b/ros3rag.common/src/main/resources/jastadd/types.relast
index 617851a..c5585b9 100644
--- a/ros3rag.common/src/main/resources/jastadd/types.relast
+++ b/ros3rag.common/src/main/resources/jastadd/types.relast
@@ -9,7 +9,7 @@ ObjectOfInterest ::= <Name:String> Position Size Orientation ;
 
 DropOffLocation : ObjectOfInterest ;
 MovableObject : ObjectOfInterest ;
-RobotObject : ObjectOfInterest ::= <Active:boolean> ;
+RobotObject : ObjectOfInterest ::= <State:de.tudresden.inf.st.ceti.Object.State> ;
 
 LogicalScene ::= LogicalRegion* LogicalMovableObject* ;
 LogicalObjectOfInterest ::= <Name:String> ;
diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.connect b/ros3rag.placeB/src/main/jastadd/WorldModelB.connect
index c207e21..2c88a86 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 Robot.CanReachObjectOfInterestWrapper using ParseReachability, ConvertReachability ;
 receive Robot.OwnedCollaborationZoneNames using ConfigChangeCommandCheckForOwnedCollaborationZone ;
 receive Robot.OccupiedCollaborationZoneNames using CommandCheckForOccupiedCollaborationZone ;
+receive Robot.CurrentPosition ;
 
 ParseReachability maps byte[] bytes to de.tudresden.inf.st.ceti.Reachability {:
   return de.tudresden.inf.st.ceti.Reachability.parseFrom(bytes);
@@ -21,6 +22,7 @@ ConvertReachability maps de.tudresden.inf.st.ceti.Reachability r to CanReachObje
 
 // --- sending ---
 send WorldModelB.NextOperation using PrintOperation ;
+send Robot.myPosition(String) ;
 
 PrintOperation maps Operation op to byte[] {:
   byte[] result = op.toProtobufByteArray();
diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd
index fd83f7a..9446b3d 100644
--- a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd
+++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd
@@ -179,10 +179,14 @@ aspect Computation {
                     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.occupient().isBusy()) {
+                        result.add(Wait.of());
+                      } else {
+                        result.add(Evacuate.of(cz.occupient(), cz));
+                      }
                     }
                     if (!cz.hasOwner() || (cz.hasOwner() && !cz.owner().equals(executingRobot))) {
-                      if (cz.hasOccupient() && !cz.occupient().equals(executingRobot)) {
+                      if (cz.hasOccupient() && (!cz.occupient().equals(executingRobot) || cz.occupient().isBusy())) {
                         result.add(Wait.of());
                       } else {
                         result.add(ConfigChange.of(executingRobot, cz));
@@ -194,10 +198,14 @@ aspect Computation {
                     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.occupient().isBusy()) {
+                        result.add(Wait.of());
+                      } else {
+                        result.add(Evacuate.of(cz.occupient(), cz));
+                      }
                     }
                     if (!cz.hasOwner() || (cz.hasOwner() && !cz.owner().equals(executingRobot))) {
-                      if (cz.hasOccupient() && !cz.occupient().equals(executingRobot)) {
+                      if (cz.hasOccupient() && (!cz.occupient().equals(executingRobot) || cz.occupient().isBusy())) {
                         result.add(Wait.of());
                       } else {
                         result.add(ConfigChange.of(executingRobot, cz));
@@ -297,6 +305,36 @@ aspect Computation {
     }
     return null;
   }
+
+  syn String Robot.myPosition() {
+    //TODO "(^)" "(>)"
+    RobotObject myRobotObject = myRobotObject();
+    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) {
+          return targetZone;  // robot was moving and has now reached its target
+        }
+        if (myRobotObject.getState() == de.tudresden.inf.st.ceti.Object.State.STATE_MOVING) {
+          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) {
+          return "";  // robot was moving and has now reached its target
+        }
+        if (myRobotObject.getState() == de.tudresden.inf.st.ceti.Object.State.STATE_MOVING) {
+          return zoneName;  // actually return (^) + targetZone
+        }
+        // robot is not moving (or picking?)
+        return targetZone;
+      }
+    }
+    return getCurrentPosition();
+  }
 }
 
 aspect AttributeMappings {
@@ -359,18 +397,17 @@ aspect Navigation {
   syn Robot CollaborationZone.occupient() {
     // (>) == moving in, (^) == evacuate/moving out
     for (Robot robot : worldModelB().getRobotList()) {
-      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;
-        }
+      String zoneName = robot.getCurrentPosition();
+      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 is moving out of this zone. also regard it as occupied
+        return robot;
+      }
+      if (zoneName.equals(getName())) {
+        return robot;
       }
     }
     return null;
@@ -397,8 +434,13 @@ aspect Navigation {
   }
 
   syn boolean Robot.isBusy() {
+    RobotObject obj = myRobotObject();
+    return obj != null && obj.getState() != de.tudresden.inf.st.ceti.Object.State.STATE_IDLE;
+  }
+
+  syn RobotObject Robot.myRobotObject() {
     ObjectOfInterest obj = worldModelB().getMyScene().resolveObjectOfInterest(getName());
-    return obj == null ? false : obj.asRobotObject().getActive();
+    return obj != null ? obj.asRobotObject() : null;
   }
 }
 
diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.relast b/ros3rag.placeB/src/main/jastadd/WorldModelB.relast
index 4626ddc..821bcca 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> <OccupiedCollaborationZoneNames> ;
+Robot ::= <Name:String> CanReachObjectOfInterestWrapper <OwnedCollaborationZoneNames> <OccupiedCollaborationZoneNames> <CurrentPosition> ;
 // relations into nodes received by RagConnect are not allowed
 //rel Robot.OwnedCollaborationZone* <-> CollaborationZone.Owner? ;
 //rel Robot.OccupiedCollaborationZone? <-> CollaborationZone.Occupient? ;
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 3a30fa3..49c01ed 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
@@ -57,17 +57,17 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> {
             e.printStackTrace();
           }
           break;
-        case "arm1/active":
-          updateAndPublishScene("arm1", robot -> robot.toBuilder().setActive(true).build());
+        case "arm1/moving":
+          updateAndPublishScene("arm1", robot -> robot.toBuilder().setState(Object.State.STATE_MOVING).build());
           break;
-        case "arm1/inactive":
-          updateAndPublishScene("arm1", robot -> robot.toBuilder().setActive(false).build());
+        case "arm1/idle":
+          updateAndPublishScene("arm1", robot -> robot.toBuilder().setState(Object.State.STATE_IDLE).build());
           break;
-        case "arm2/active":
-          updateAndPublishScene("arm2", robot -> robot.toBuilder().setActive(true).build());
+        case "arm2/moving":
+          updateAndPublishScene("arm2", robot -> robot.toBuilder().setState(Object.State.STATE_MOVING).build());
           break;
-        case "arm2/inactive":
-          updateAndPublishScene("arm2", robot -> robot.toBuilder().setActive(false).build());
+        case "arm2/idle":
+          updateAndPublishScene("arm2", robot -> robot.toBuilder().setState(Object.State.STATE_IDLE).build());
           break;
         case "big-blue/cz":
           updateAndPublishScene("bigBlue", bigBlue -> {
@@ -140,6 +140,10 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> {
       // self-loop
       robot.connectOwnedCollaborationZoneNames(mqttUri(config.forB.topicCommand, config));
       robot.connectOccupiedCollaborationZoneNames(mqttUri(config.forB.topicCommand, config));
+
+      String topicPosition = joinTopics("place-b", robot.getName(), "position");
+      robot.connectCurrentPosition(mqttUri(topicPosition, config));
+      robot.connectMyPosition(mqttUri(topicPosition, config), true);
     }
   }
 
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 a1b38e8..a83b918 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
@@ -259,7 +259,7 @@ public class SimpleMainB {
   static de.tudresden.inf.st.ceti.Scene updateNotBusyOfRobot(
       de.tudresden.inf.st.ceti.Scene scene,
       String objectName) {
-    return UtilB.updateObject(scene, objectName, obj -> obj.toBuilder().setActive(false).build());
+    return UtilB.updateObject(scene, objectName, obj -> obj.toBuilder().setState(Object.State.STATE_MOVING).build());
   }
 
 
diff --git a/ros3rag.placeB/src/main/resources/config-scene-b-placeworld-manual.json b/ros3rag.placeB/src/main/resources/config-scene-b-placeworld-manual.json
index 0de3c1e..23a488c 100644
--- a/ros3rag.placeB/src/main/resources/config-scene-b-placeworld-manual.json
+++ b/ros3rag.placeB/src/main/resources/config-scene-b-placeworld-manual.json
@@ -1498,7 +1498,8 @@
         "r": 1,
         "g": 1,
         "b": 1
-      }
+      },
+      "state": "STATE_IDLE"
     },
     {
       "id": "arm2",
@@ -1516,7 +1517,8 @@
         "r": 1,
         "g": 1,
         "b": 1
-      }
+      },
+      "state": "STATE_IDLE"
     }
   ]
 }
-- 
GitLab