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