diff --git a/cloc/.gitignore b/cloc/.gitignore index c23149d43d0605af404b06376914060ee2bb7d30..94a635291ffb675001e680c7785f8ba6f2a3e4b0 100644 --- a/cloc/.gitignore +++ b/cloc/.gitignore @@ -4,3 +4,5 @@ acs.txt cfcs.txt trainbenchmark.file trainbenchmark.lang +cats.txt +ignored.txt diff --git a/cloc/cloc-def-connect.txt b/cloc/cloc-def-connect.txt new file mode 100644 index 0000000000000000000000000000000000000000..a20ca00ecfd5c2377ea06a87ebda2c4731ac30a2 --- /dev/null +++ b/cloc/cloc-def-connect.txt @@ -0,0 +1,5 @@ +Connect + filter call_regexp_common Java + extension connect + extension ragconnect + 3rd_gen_scale 10 diff --git a/cloc/run-cloc.sh b/cloc/run-cloc.sh index 56f3d936b2104e98beb324036d90630975fcae53..d17af8bd03a8286e41278e622d7c5b4cbc8d3cda 100755 --- a/cloc/run-cloc.sh +++ b/cloc/run-cloc.sh @@ -20,14 +20,14 @@ done echo "Running cloc with new definitions" # --ignored=bad-files.txt CLOC_CMD="cloc --exclude-lang=JSON --read-lang-def=my_definitions.txt --exclude-list-file=.clocignore --quiet" -$CLOC_CMD --report-file=common-result.txt ../ros3rag.common/src/main/resources/jastadd ../ros3rag.common/src/main/java 2>>cloc-errors.log -$CLOC_CMD --report-file=placeA-result.txt ../ros3rag.placeA/src/main/ja* 2>>cloc-errors.log -$CLOC_CMD --report-file=placeB-result.txt ../ros3rag.placeB/src/main/ja* 2>>cloc-errors.log +$CLOC_CMD --report-file=common-01-input-result.txt ../ros3rag.common/src/main/resources/jastadd ../ros3rag.common/src/main/proto ../ros3rag.common/src/main/java 2>>cloc-errors.log +$CLOC_CMD --report-file=placeA-01-input-result.txt ../ros3rag.placeA/src/main/ja* 2>>cloc-errors.log +$CLOC_CMD --report-file=placeB-01-input-result.txt --categorized=cats.txt --ignored=ignored.txt ../ros3rag.placeB/src/main/ja* 2>>cloc-errors.log # $CLOC_CMD --report-file=base-gen-result.txt ../ros3rag.base/src/gen 2>>cloc-errors.log -$CLOC_CMD --report-file=placeA-gen-result.txt ../ros3rag.placeA/src/gen 2>>cloc-errors.log -$CLOC_CMD --report-file=placeB-gen-result.txt ../ros3rag.placeB/src/gen 2>>cloc-errors.log -$CLOC_CMD --report-file=common-gen-result.txt ../ros3rag.common/build/generated/source/proto/main/java/de 2>>cloc-errors.log +$CLOC_CMD --report-file=placeA-02-gen-result.txt ../ros3rag.placeA/src/gen 2>>cloc-errors.log +$CLOC_CMD --report-file=placeB-02-gen-result.txt ../ros3rag.placeB/src/gen 2>>cloc-errors.log +$CLOC_CMD --report-file=common-02-gen-result.txt ../ros3rag.common/build/generated/source/proto/main/java/de 2>>cloc-errors.log # CFC_CMD='grep -o 'if'\|'for'\|'return'' 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 89e81de99794927a3f6e516f9571fe525f75b417..5951e02070cbc6af009932eb27f40c603d4c18f9 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 @@ -60,7 +60,10 @@ public abstract class SharedMainParts<MqttHandler extends SharedMainParts.MqttHa /// Prepare main handler mainHandler = createMqttHandler().dontSendWelcomeMessage(); mainHandler.setHost(config.mqttHost); - mainHandler.waitUntilReady(2, TimeUnit.SECONDS); + if (!mainHandler.waitUntilReady(2, TimeUnit.SECONDS)) { + logger.fatal("Could not connect to mqtt handler. Exiting"); + return; + } CountDownLatch exitCondition = new CountDownLatch(1); mainHandler.newConnection(TOPIC_EXIT, bytes -> { logger.info("Got exit command"); diff --git a/ros3rag.common/src/main/proto/config.proto b/ros3rag.common/src/main/proto/config.proto deleted file mode 100644 index 38cb5d0a25b02c96dcbdf30f685212ea972feee6..0000000000000000000000000000000000000000 --- a/ros3rag.common/src/main/proto/config.proto +++ /dev/null @@ -1,7 +0,0 @@ -syntax = "proto3"; - -package config; - -message RobotConfig { - double speed = 1; -} \ No newline at end of file diff --git a/ros3rag.common/src/main/proto/robot_state.proto b/ros3rag.common/src/main/proto/robot_state.proto deleted file mode 100644 index 6630631f16e00493be3f50307c9092e56184e9c6..0000000000000000000000000000000000000000 --- a/ros3rag.common/src/main/proto/robot_state.proto +++ /dev/null @@ -1,37 +0,0 @@ -syntax = "proto3"; - -package robot; - -message RobotState { - - message Position { - double x = 1; - double y = 2; - double z = 3; - } - - message Orientation { - double x = 1; - double y = 2; - double z = 3; - double w = 4; - } - - message LinearTwist { - double x = 1; - double y = 2; - double z = 3; - } - - message AngularTwist { - double x = 1; - double y = 2; - double z = 3; - } - - string name = 1; - Position position = 2; - Orientation orientation = 3; - LinearTwist linear_twist = 4; - AngularTwist angular_twist = 5; -} \ No newline at end of file diff --git a/ros3rag.common/src/main/proto/trajectory.proto b/ros3rag.common/src/main/proto/trajectory.proto deleted file mode 100644 index 4a7f375e1ae37ba386f45e1149dc38990d0c7a0e..0000000000000000000000000000000000000000 --- a/ros3rag.common/src/main/proto/trajectory.proto +++ /dev/null @@ -1,33 +0,0 @@ -syntax = "proto3"; - -package plan; - -message Trajectory { - - message Position { - double x = 1; - double y = 2; - double z = 3; - } - - message Orientation { - double x = 1; - double y = 2; - double z = 3; - double w = 4; - } - - enum PlanningMode { - FLUID = 0; - CARTESIAN = 1; - } - - message Pose { - Position position = 1; - Orientation orientation = 2; - PlanningMode mode = 3; - } - - repeated Pose pose = 1; - bool loop = 2; -} diff --git a/ros3rag.common/src/main/resources/jastadd/types.connect b/ros3rag.common/src/main/resources/jastadd/types.connect index 28bc80d7f9d7b429feccf9c933bd95fd3943c2ee..911770fd3b4f28b93c91a2753093b2dfea77ee88 100644 --- a/ros3rag.common/src/main/resources/jastadd/types.connect +++ b/ros3rag.common/src/main/resources/jastadd/types.connect @@ -62,17 +62,3 @@ ConvertScene maps de.tudresden.inf.st.ceti.Scene pbScene to Scene {: } return result; :} - -ParseReachability maps byte[] bytes to de.tudresden.inf.st.ceti.Reachability {: - return de.tudresden.inf.st.ceti.Reachability.parseFrom(bytes); -:} - -ConvertReachability maps de.tudresden.inf.st.ceti.Reachability r to CanReachObjectOfInterestWrapper {: - var result = new CanReachObjectOfInterestWrapper(); - for (var objReach : r.getObjectsList()) { - if (objReach.getReachable()) { - result.addCanReachObjectOfInterest(new CanReachObjectOfInterest().setObjectName(objReach.getIdObject())); - } - } - return result; -:} diff --git a/ros3rag.common/src/main/resources/jastadd/types.jadd b/ros3rag.common/src/main/resources/jastadd/types.jadd index 11404ecf542e3c1ad69bf2cd0c5613a3eec0d03e..8ee7d98d85588cca63675ff54ac47761ac74dbce 100644 --- a/ros3rag.common/src/main/resources/jastadd/types.jadd +++ b/ros3rag.common/src/main/resources/jastadd/types.jadd @@ -165,14 +165,6 @@ aspect Navigation { syn LogicalMovableObject LogicalObjectOfInterest.asLogicalMovableObject() = null; eq LogicalMovableObject.asLogicalMovableObject() = this; - // --- isCollaborationZone --- - syn boolean DropOffLocation.isCollaborationZone() = false; - eq CollaborationZone.isCollaborationZone() = true; - - // --- asCollaborationZone --- - syn CollaborationZone DropOffLocation.asCollaborationZone() = null; - eq CollaborationZone.asCollaborationZone() = this; - inh Scene DropOffLocation.containingScene(); inh Scene MovableObject.containingScene(); inh Scene LogicalMovableObject.containingScene(); diff --git a/ros3rag.common/src/main/resources/jastadd/types.relast b/ros3rag.common/src/main/resources/jastadd/types.relast index 8061ea582af15f8eb156926916f36135f364b51c..617851a05511e6ca0d3dfc4cbca87f1ac7f2dd30 100644 --- a/ros3rag.common/src/main/resources/jastadd/types.relast +++ b/ros3rag.common/src/main/resources/jastadd/types.relast @@ -5,26 +5,23 @@ Orientation ::= <X:double> <Y:double> <Z:double> <W:double> ; // Regions cannot be contained in scene, but must be retrieved via attribute Scene ::= DropOffLocation* MovableObject* RobotObject* /LogicalScene/ ; -CanReachObjectOfInterestWrapper ::= CanReachObjectOfInterest* ; ObjectOfInterest ::= <Name:String> Position Size Orientation ; DropOffLocation : ObjectOfInterest ; MovableObject : ObjectOfInterest ; RobotObject : ObjectOfInterest ::= <Active:boolean> ; -CanReachObjectOfInterest ::= <ObjectName:String> ; - LogicalScene ::= LogicalRegion* LogicalMovableObject* ; LogicalObjectOfInterest ::= <Name:String> ; LogicalRegion : LogicalObjectOfInterest ; LogicalMovableObject : LogicalObjectOfInterest ::= <NameOfMyLocation> ; rel LogicalRegion.ContainedObject* <-> LogicalMovableObject.LocatedAt* ; -// TODO could lead to problems when including this information and sending it +// could lead to problems when including this information and sending it //rel LogicalMovableObject.MyLocation? -> DropOffLocation ; Region ::= <Name:String> <LocationNames> ; //rel Region.Location* <-> DropOffLocation.ContainedInRegion* ; -// TODO should this be only in site-B?? +// only used in site-B, but needed here for parsing the scene CollaborationZone : DropOffLocation ; diff --git a/ros3rag.placeA/src/main/resources/config-a-placeworld.yaml b/ros3rag.placeA/src/main/resources/config-a-placeworld.yaml index 19e4029079f1fd7f572790f9609b0ae7924b52c5..32c3502e03b077ca3548d705ede80b8052bcc691 100644 --- a/ros3rag.placeA/src/main/resources/config-a-placeworld.yaml +++ b/ros3rag.placeA/src/main/resources/config-a-placeworld.yaml @@ -1,4 +1,5 @@ -mqttHost: "192.168.0.122" +#mqttHost: "192.168.0.122" +mqttHost: "localhost" filenameRegions: "src/main/resources/regions-a-placeworld.json" forA: filenameInitialScene: "src/main/resources/config-scene-a-placeworld.json" diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.connect b/ros3rag.placeB/src/main/jastadd/WorldModelB.connect index de33b2fb9ca0e42a29b5969d137f1345aceded8b..7ed992b8182365883a8a90e8d91d09d1f7080251 100644 --- a/ros3rag.placeB/src/main/jastadd/WorldModelB.connect +++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.connect @@ -6,6 +6,20 @@ receive Robot.CanReachObjectOfInterestWrapper using ParseReachability, ConvertRe receive Robot.OwnedCollaborationZoneNames using ConfigChangeCommandCheckForOwnedCollaborationZone ; receive Robot.OccupiedCollaborationZoneNames using CommandCheckForOccupiedCollaborationZone ; +ParseReachability maps byte[] bytes to de.tudresden.inf.st.ceti.Reachability {: + return de.tudresden.inf.st.ceti.Reachability.parseFrom(bytes); +:} + +ConvertReachability maps de.tudresden.inf.st.ceti.Reachability r to CanReachObjectOfInterestWrapper {: + var result = new CanReachObjectOfInterestWrapper(); + for (var objReach : r.getObjectsList()) { + if (objReach.getReachable()) { + result.addCanReachObjectOfInterest(new CanReachObjectOfInterest().setObjectName(objReach.getIdObject())); + } + } + return result; +:} + // --- sending --- send WorldModelB.NextOperation using PrintOperation ; diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd index 6142c563f4814856eee6bfbb74d7035e5457a1cb..fd83f7a3eef7a5314d5711f4e6f6af079b6a51d6 100644 --- a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd +++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd @@ -335,6 +335,14 @@ aspect AttributeMappings { } aspect Navigation { + // --- isCollaborationZone --- + syn boolean DropOffLocation.isCollaborationZone() = false; + eq CollaborationZone.isCollaborationZone() = true; + + // --- asCollaborationZone --- + syn CollaborationZone DropOffLocation.asCollaborationZone() = null; + eq CollaborationZone.asCollaborationZone() = this; + //--- worldModelB --- inh WorldModelB ASTNode.worldModelB(); eq WorldModelB.getChild().worldModelB() = this; diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.relast b/ros3rag.placeB/src/main/jastadd/WorldModelB.relast index 7dc16b58a3adb12aba2ada75660cd825c1098b09..4626ddce9e640fa7b64b5cb8ece30e57e542b63a 100644 --- a/ros3rag.placeB/src/main/jastadd/WorldModelB.relast +++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.relast @@ -6,6 +6,9 @@ Robot ::= <Name:String> CanReachObjectOfInterestWrapper <OwnedCollaborationZoneN //rel Robot.OwnedCollaborationZone* <-> CollaborationZone.Owner? ; //rel Robot.OccupiedCollaborationZone? <-> CollaborationZone.Occupient? ; +CanReachObjectOfInterestWrapper ::= CanReachObjectOfInterest* ; +CanReachObjectOfInterest ::= <ObjectName:String> ; + abstract Difference ; rel Difference.Object -> LogicalMovableObject ; DifferenceObjectAtWrongPlace : Difference ; 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 027516d742e7cd40a2ecba6edd2816bf0b8e3823..3a30fa387bb52f58f4ec02e36b4bd2511d47c309 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 @@ -22,7 +22,6 @@ 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_DEMO_MOVE_objectRed1_RED; private final String TOPIC_MODEL_SVG_PATH; private de.tudresden.inf.st.ceti.Scene demo_scene; @@ -30,7 +29,6 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> { MainB(String configFile) { super("place-b", UtilB.pathToDirectoryOfPlaceB().resolve(configFile)); - this.TOPIC_DEMO_MOVE_objectRed1_RED = cellName + "/demo/move/objectRed1/red"; this.TOPIC_MODEL_SVG_PATH = cellName + "/model/svg/path"; } @@ -46,49 +44,59 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> { @Override protected void createSpecificMainHandlerConnections() { - 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("place-b/demo", bytes -> { + switch (new String(bytes)) { + case "objectRed1/red": + UtilB.updatePositionOfObjectToLocation(model.getMyScene(), "objectRed1", "binRed"); + break; + case "initial_scene": + 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(); + } + break; + case "arm1/active": + updateAndPublishScene("arm1", robot -> robot.toBuilder().setActive(true).build()); + break; + case "arm1/inactive": + updateAndPublishScene("arm1", robot -> robot.toBuilder().setActive(false).build()); + break; + case "arm2/active": + updateAndPublishScene("arm2", robot -> robot.toBuilder().setActive(true).build()); + break; + case "arm2/inactive": + updateAndPublishScene("arm2", robot -> robot.toBuilder().setActive(false).build()); + break; + case "big-blue/cz": + updateAndPublishScene("bigBlue", bigBlue -> { + Object.Builder builder = bigBlue.toBuilder(); + builder.getPosBuilder() + .setX(0.0012) + .setY(0.65) + .setZ(0.94); + return builder.build(); + }); + break; + case "big-blue/g1": + updateAndPublishScene("bigBlue", bigBlue -> { + Object.Builder builder = bigBlue.toBuilder(); + builder.getPosBuilder() + .setX(0.1) + .setY(1.0) + .setZ(0.93); + return builder.build(); + }); + break; + default: + logger.error("Unknown demo command {}", new String(bytes)); } }); - 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); + demo_scene = UtilB.updateObject(demo_scene, objectName, change); mainHandler.publish(config.forB.topicsSceneUpdate.get(0), demo_scene.toByteArray()); } diff --git a/ros3rag.placeB/src/main/resources/config-b-placeworld.yaml b/ros3rag.placeB/src/main/resources/config-b-placeworld.yaml index 72a4db52f7b5a164e358f589c14dec22e1f8592d..cd50a130378e62e501c2d8f429533bba5a52a13c 100644 --- a/ros3rag.placeB/src/main/resources/config-b-placeworld.yaml +++ b/ros3rag.placeB/src/main/resources/config-b-placeworld.yaml @@ -1,4 +1,5 @@ -mqttHost: "192.168.0.122" +#mqttHost: "192.168.0.122" +mqttHost: "localhost" filenameRegions: "src/main/resources/regions-b-placeworld.json" forB: topicsSceneUpdate: