WIP: updated protobuf, working on correct robot position handling

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

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;

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> ;

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();

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)) {
+                      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)) {
+                      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,20 +397,19 @@ aspect Navigation {
   syn Robot CollaborationZone.occupient() {
     // (>) == moving in, (^) == evacuate/moving out
     for (Robot robot : worldModelB().getRobotList()) {
-      for (String zoneName : arrayAsList(robot.getOccupiedCollaborationZoneNames())) {
+      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.isBusy()) {
-          // robot is busy moving out of this zone. regard it as occupied as long as the robot is busy
+      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;
   }
   syn boolean CollaborationZone.hasOwner() = owner() != 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;
   }
 }
 

ros3rag.placeB/src/main/jastadd/WorldModelB.relast

 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? ;

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);
     }
   }
 

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());
   }
 
 

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"
     }
   ]
 }