diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/lerobot/RDXVLAOperation.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/lerobot/RDXVLAOperation.java index ddf36500d35..93883de74ab 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/lerobot/RDXVLAOperation.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/lerobot/RDXVLAOperation.java @@ -273,4 +273,9 @@ private void getRenderables(Array renderables, Pool pool actionForearmPoseGraphics.get(side).getRenderables(renderables, pool); } } + + public CRDTBidirectionalBoolean getRunning() + { + return running; + } } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/lerobot/VLAUpdateThread.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/lerobot/VLAUpdateThread.java index c637026b104..0b376478f0a 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/lerobot/VLAUpdateThread.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/lerobot/VLAUpdateThread.java @@ -78,6 +78,7 @@ public class VLAUpdateThread extends VLAYoRegistry private final LatestTimestampModifiable latestTimestampModifiable; private long sequenceID = 0L; private final CRDTBidirectionalBoolean running; + private boolean keepRunning = true; private final CRDTBidirectionalBoolean controlRobot; private final TypedNotification uiCommandSubscription; private final ROS2Publisher uiStatusPublisher; @@ -124,7 +125,7 @@ public void runTask() { handUISyncBefore(); - if (running.getValue()) + if (keepRunning && running.getValue()) { if (openpiRequest == null) { @@ -153,6 +154,7 @@ else if (openpiRequest.isDone()) DoubleBuffer action = actionPlan.pollFirst(); if (action != null) { + keepRunning = false; dispatchActionToKST(action); } } @@ -396,6 +398,8 @@ private void handUISyncBefore() ToolboxStateMessage toolboxStateMessage = new ToolboxStateMessage(); toolboxStateMessage.setRequestedToolboxState(ToolboxState.WAKE_UP.toByte()); kstStatePublisher.publish(toolboxStateMessage); + + keepRunning = true; } controlRobot.fromMessage(uiCommand.getControlRobot()); } diff --git a/ihmc-perception/src/main/java/us/ihmc/sensors/ImageSensor.java b/ihmc-perception/src/main/java/us/ihmc/sensors/ImageSensor.java index 4d4c2fec742..b5fcd127fa9 100644 --- a/ihmc-perception/src/main/java/us/ihmc/sensors/ImageSensor.java +++ b/ihmc-perception/src/main/java/us/ihmc/sensors/ImageSensor.java @@ -171,7 +171,7 @@ public RepeatingTaskThread getGrabThread() return grabThread; } - private void grabAndNotify() + public void grabAndNotify() { // If the sensor is not running, try to start the sensor if (!isSensorRunning() && !startSensor()) diff --git a/ihmc-perception/src/main/java/us/ihmc/sensors/zed/ZEDSVOPlaybackSensor.java b/ihmc-perception/src/main/java/us/ihmc/sensors/zed/ZEDSVOPlaybackSensor.java index fdf8bb40342..547e228f94b 100644 --- a/ihmc-perception/src/main/java/us/ihmc/sensors/zed/ZEDSVOPlaybackSensor.java +++ b/ihmc-perception/src/main/java/us/ihmc/sensors/zed/ZEDSVOPlaybackSensor.java @@ -28,6 +28,12 @@ public boolean startSensor() return super.startSensor(); } + @Override + public boolean grab() + { + return super.grab(); + } + public void play() { run(true);