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 dfb03f3f94b8c06192e181b52f23713d597b8406..b1127e16ae6910bfcd9973eb3ca419181c37f286 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 @@ -68,7 +68,12 @@ public abstract class SharedMainParts<MqttHandler extends SharedMainParts.MqttHa mainHandler.setHost(config.mqttHost); mainHandler.waitUntilReady(2, TimeUnit.SECONDS); CountDownLatch exitCondition = new CountDownLatch(1); - mainHandler.newConnection(TOPIC_EXIT, bytes -> exitCondition.countDown()); + mainHandler.newConnection(TOPIC_EXIT, bytes -> { + logger.info("Got exit command"); + exitCondition.countDown(); + logger.debug("exit latch count = {}, start latch count= {}", + exitCondition.getCount(), startCondition.getCount()); + }); mainHandler.newConnection(TOPIC_MODEL, bytes -> logStatus(new String(bytes))); mainHandler.newConnection(TOPIC_REWIND, bytes -> { @@ -115,6 +120,7 @@ public abstract class SharedMainParts<MqttHandler extends SharedMainParts.MqttHa } private void reactToCoordinatorCommand(String command) { + logger.debug("Got coordinator command {} for {}", command, cellName); switch (command) { case "rewind": try { @@ -127,7 +133,7 @@ public abstract class SharedMainParts<MqttHandler extends SharedMainParts.MqttHa startCondition.countDown(); break; default: - System.err.println("Unknown coordinator command: " + command); + logger.warn("Unknown coordinator command {} for {}", command, cellName); } } @@ -135,29 +141,35 @@ public abstract class SharedMainParts<MqttHandler extends SharedMainParts.MqttHa private void rewind(String statusMessage) throws Exception { if (model != null) { + logger.debug("Closing previous connections for {}", cellName); model.ragconnectCloseConnections(); } + logger.debug("Creating world model for {}", cellName); model = createWorldModel(); + logger.debug("Reading scene for {}", cellName); Scene scene = readSceneAndRobots(); - /// Setup model connection + logger.debug("Setup model connection for {}", cellName); model.ragconnectCheckIncremental(); model.ragconnectSetupMqttWaitUntilReady(2, TimeUnit.SECONDS); if (config.useCoordinator()) { + logger.debug("Awaiting start signal from coordinator for {}", cellName); startCondition = new CountDownLatch(1); mainHandler.publish(joinTopics(config.coordinatorMqttTopicPrefix, TOPIC_SUFFIX_COORDINATOR_STATUS), "up".getBytes(StandardCharsets.UTF_8)); startCondition.await(); } + logger.debug("Connecting endpoints for {}", cellName); connectEndpoints(); logStatus(statusMessage); mainHandler.publish(TOPIC_SCENE_INIT, scene.toByteArray()); if (config.useCoordinator()) { + logger.debug("Publishing ready for {}", cellName); mainHandler.publish(joinTopics(config.coordinatorMqttTopicPrefix, TOPIC_SUFFIX_COORDINATOR_STATUS), "ready".getBytes(StandardCharsets.UTF_8)); } diff --git a/ros3rag.common/src/main/resources/tasks.gradle b/ros3rag.common/src/main/resources/tasks.gradle index 8016213bdb698da3a2458d150e84f6574ec4f03e..8af9da6e89a4be3a0dea360a6d5acecf1807e064 100644 --- a/ros3rag.common/src/main/resources/tasks.gradle +++ b/ros3rag.common/src/main/resources/tasks.gradle @@ -23,6 +23,7 @@ task ragConnect(type: JavaExec) { new File(project.ext.sharedJastAddDir, 'types.connect'), '--logReads', '--logWrites', +// '--logIncremental', // '--verbose', '--rootNode=' + project.ext.ragConnectRootNode, '--List=JastAddList', 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 930b0480187c00b0228164e97e392af1ceca118a..88ca30aea2761657ebabae442a1e8b95ef474a93 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 @@ -82,7 +82,7 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> { @Override protected void connectEndpoints() throws IOException { model.connectMyScene(mqttUri(TOPIC_MY_SCENE_UPDATE_FROM_ROS, config)); - model.connectOtherScene(mqttUri(TOPIC_OTHER_SCENE_UPDATE_FROM_PLACE_A, config)); + model.connectOtherScene(mqttUri(TOPIC_OTHER_SCENE_UPDATE_FROM_PLACE_A, config), 0); model.connectNextOperation(mqttUri(TOPIC_COMMAND, config), false); } @@ -93,7 +93,9 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> { @Override protected WorldModelB createWorldModel() { - return new WorldModelB(); + WorldModelB result = new WorldModelB(); + result.addOtherScene(new LogicalScene()); + return result; } @Override diff --git a/ros3rag.placeB/src/main/resources/config-b.yaml b/ros3rag.placeB/src/main/resources/config-b.yaml index 657f5ba3915b109991428609720b061f7aa45f12..5f1f4ebc981fa7420f1598f53ba143391722b7b3 100644 --- a/ros3rag.placeB/src/main/resources/config-b.yaml +++ b/ros3rag.placeB/src/main/resources/config-b.yaml @@ -1,6 +1,6 @@ mqttHost: "localhost" filenameInitialScene: "src/main/resources/config-scene-b.json" -useReachability: false +useReachability: true reachability: - idRobot: "arm1" filename: "src/main/resources/dummy-reachability-b-arm1.json"