fix(manipulation): detect Viser execution completion and consume plans

dimos/manipulation/manipulation_module.py

@@ -374,6 +374,7 @@ def _tf_publish_loop(self) -> None:
     @rpc
     def get_state(self) -> ManipulationSnapshot:
         """Return one snapshot containing every planning group."""
+        self._refresh_execution_status()
         groups: dict[PlanningGroupID, PlanningGroupState] = {}
         if self._world_monitor is not None:
             for group in self._world_monitor.planning_groups.list():
@@ -414,9 +415,19 @@ def get_state(self) -> ManipulationSnapshot:
 
     def get_operation_status(self) -> OperationStatus:
         """Return the current operation status without collecting telemetry."""
+        self._refresh_execution_status()
         with self._lock:
             return OperationStatus[self._state.name]
 
+    def _refresh_execution_status(self) -> None:
+        """Poll active nonblocking execution once, without waiting for motion."""
+        manager = getattr(self, "_execution_manager", None)
+        if manager is not None and manager.status in {
+            ExecutionStatus.ACCEPTED,
+            ExecutionStatus.EXECUTING,
+        }:
+            self.wait_for_execution(timeout=0.0)
+
     @rpc
     def get_error(self) -> str:
         """Get last error message.

dimos/manipulation/test_plan_execution.py

@@ -16,6 +16,8 @@
 
 from unittest.mock import MagicMock
 
+import pytest
+
 from dimos.control.coordinator import ControlCoordinator
 from dimos.control.tasks.trajectory_task.trajectory_task import (
     TrajectoryCancellationResult,
@@ -27,9 +29,11 @@
 from dimos.manipulation.manipulation_spec import ExecutionStatus
 from dimos.manipulation.planning.spec.enums import PlanningStatus
 from dimos.manipulation.planning.spec.models import GeneratedPlan
+from dimos.manipulation.visualization.operator import ManipulationOperator
 from dimos.msgs.sensor_msgs.JointState import JointState
 from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory
 from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint
+from dimos.msgs.trajectory_msgs.TrajectoryStatus import TrajectoryState, TrajectoryStatus
 
 
 def _plan(final_position: float = 1.0) -> GeneratedPlan:
@@ -129,3 +133,50 @@ def test_uncertain_cancel_projects_to_fault(module_factory) -> None:
 
     assert module._state is ManipulationState.FAULT
     assert "timed out" in module.get_error()
+
+
+@pytest.mark.parametrize("reader", ["operator", "snapshot"])
+@pytest.mark.parametrize(
+    ("terminal", "operation"),
+    [
+        (TrajectoryState.COMPLETED, "COMPLETED"),
+        (TrajectoryState.ABORTED, "IDLE"),
+        (TrajectoryState.FAULT, "FAULT"),
+    ],
+)
+def test_status_refresh_observes_nonblocking_execution(module_factory, reader, terminal, operation):
+    coordinator = _coordinator()
+    module = module_factory(coordinator)
+    operator = ManipulationOperator(module, MagicMock())
+
+    def read_status():
+        if reader == "operator":
+            return operator.status().state
+        return module.get_state().operation_status.name
+
+    assert operator.execute(_plan()) is True
+    coordinator.task_invoke.return_value = TrajectoryStatus(state=TrajectoryState.EXECUTING)
+    assert read_status() == "EXECUTING"
+
+    coordinator.task_invoke.return_value = TrajectoryStatus(state=terminal)
+    assert read_status() == operation
+    assert module.get_state().execution_status.name == terminal.name
+    coordinator.task_invoke.reset_mock()
+    assert read_status() == operation
+    coordinator.task_invoke.assert_not_called()
+
+    assert operator.execute(_plan()) is True
+
+
+def test_status_refresh_reports_coordinator_failure(module_factory):
+    coordinator = _coordinator()
+    module = module_factory(coordinator)
+    operator = ManipulationOperator(module, MagicMock())
+    assert operator.execute(_plan()) is True
+    coordinator.task_invoke.side_effect = TimeoutError("status unavailable")
+
+    status = operator.status()
+
+    assert status.state == "FAULT"
+    assert "status unavailable" in status.error
+    assert module.get_state().execution_status is ExecutionStatus.UNCERTAIN

dimos/manipulation/visualization/viser/gui.py

@@ -307,7 +307,10 @@ def preview_path(self) -> bool:
 
     def execute(self) -> bool:
         plan = self.state.plan_state.plan
-        return plan is not None and self.operator.execute(plan)
+        if plan is None:
+            return False
+        self.state.plan_state = PanelPlanState()
+        return self.operator.execute(plan)
 
     def refresh(self) -> None:
         if self._closed:
@@ -1255,15 +1258,12 @@ def operation() -> None:
                 )
                 return
             self.state.action_status = ActionStatus.EXECUTING
-            self.state.plan_state.status = PlanStatus.EXECUTING
             ok = self.execute()
             if not self._operation_is_current(operation_id, selection_epoch, target_sequence_id):
                 self._finish_operation(
                     "execute=False", operation_id=operation_id, selection_epoch=selection_epoch
                 )
                 return
-            if not ok:
-                self.state.plan_state.status = PlanStatus.FAILED
             self._finish_operation(
                 f"execute={ok}", operation_id=operation_id, selection_epoch=selection_epoch
             )
@@ -1275,12 +1275,11 @@ def operation() -> None:
     def _submit_cancel(self) -> None:
         if self._closed:
             return
-        cancelled_action = self.state.action_status
         operation_id = self._next_operation_id()
         if not self._operation_is_current(operation_id):
             return
         self.state.action_status = ActionStatus.CANCELLING
-        self._mark_cancelled_plan_state(cancelled_action)
+        self._mark_cancelled_plan_state()
         self._restart_operation_worker()
         try:
             ok = self.cancel()
@@ -1289,14 +1288,9 @@ def _submit_cancel(self) -> None:
             return
         self._finish_operation(f"cancel={ok}", operation_id=operation_id)
 
-    def _mark_cancelled_plan_state(self, cancelled_action: ActionStatus) -> None:
+    def _mark_cancelled_plan_state(self) -> None:
         if self.state.plan_state.status == PlanStatus.PLANNING:
             self.state.plan_state.status = PlanStatus.FAILED
-        elif (
-            cancelled_action == ActionStatus.EXECUTING
-            or self.state.plan_state.status == PlanStatus.EXECUTING
-        ):
-            self.state.plan_state.status = PlanStatus.STALE
 
     def _restart_operation_worker(self) -> None:
         self._operation_worker.stop(timeout=0.0)

dimos/manipulation/visualization/viser/state.py

@@ -66,7 +66,6 @@ class PlanStatus(str, Enum):
     PLANNING = "planning"
     FRESH = "fresh"
     STALE = "stale"
-    EXECUTING = "executing"
     FAILED = "failed"
 
 

dimos/manipulation/visualization/viser/test_gui.py

@@ -24,22 +24,30 @@
 
 from dimos.manipulation.planning.groups.models import PlanningGroup
 from dimos.manipulation.planning.spec.config import RobotModelConfig
+from dimos.manipulation.planning.spec.enums import PlanningStatus
 from dimos.manipulation.planning.spec.models import GeneratedPlan, PlanningSceneInfo
-from dimos.manipulation.visualization.operator import OperatorStatus, TargetEvaluationResult
+from dimos.manipulation.visualization.operator import (
+    ManipulationOperator,
+    OperatorStatus,
+    TargetEvaluationResult,
+)
 from dimos.manipulation.visualization.viser.config import ViserVisualizationConfig
 from dimos.manipulation.visualization.viser.gui import ViserPanelGui
 from dimos.manipulation.visualization.viser.state import (
     ActionStatus,
     BackendConnectionStatus,
     FeasibilityStatus,
     OperationWorker,
+    PanelPlanState,
     PanelRuntime,
     PlanStatus,
     TargetEvaluationWorker,
     TargetStatus,
 )
 from dimos.msgs.sensor_msgs.JointState import JointState
 from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory
+from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint
+from dimos.msgs.trajectory_msgs.TrajectoryStatus import TrajectoryState, TrajectoryStatus
 from dimos.robot.assets.model import RobotModel
 
 
@@ -176,6 +184,136 @@ def make_gui(module: FakeOperatorBackend | None = None) -> ViserPanelGui:
     )
 
 
+@pytest.fixture
+def executable_gui(monkeypatch, mocker):
+    gui = make_gui()
+    submissions = []
+    mocker.patch.object(
+        gui._operation_worker,
+        "submit",
+        side_effect=lambda operation, **kwargs: submissions.append(operation),
+    )
+    monkeypatch.setattr(gui, "refresh", lambda: None)
+    gui.state.runtime = PanelRuntime.RUNNING
+    gui.state.backend_status = BackendConnectionStatus.READY
+    gui.state.target_status = TargetStatus.FEASIBLE
+    gui.state.manipulation_state = "COMPLETED"
+    gui.state.selected_group_ids = ("manipulator",)
+    gui.state.plan_state = PanelPlanState(
+        status=PlanStatus.FRESH,
+        group_ids=gui.state.selected_group_ids,
+        plan=GeneratedPlan(
+            group_ids=gui.state.selected_group_ids,
+            trajectory=JointTrajectory(
+                joint_names=["arm/j0"],
+                points=[
+                    TrajectoryPoint(positions=[0.0], time_from_start=0.0),
+                    TrajectoryPoint(positions=[1.0], time_from_start=1.0),
+                ],
+            ),
+            path=[JointState(name=["arm/j0"], position=[value]) for value in (0.0, 1.0)],
+            status=PlanningStatus.SUCCESS,
+        ),
+    )
+    execute = mocker.patch.object(gui.operator, "execute", create=True)
+    try:
+        yield gui, submissions, execute
+    finally:
+        gui.close()
+
+
+@pytest.mark.parametrize("accepted", [True, False])
+def test_execute_consumes_plan_before_dispatch(executable_gui, accepted):
+    gui, submissions, execute = executable_gui
+    plan = gui.state.plan_state.plan
+
+    def dispatch(dispatched_plan):
+        assert dispatched_plan is plan
+        assert gui.state.plan_state == PanelPlanState()
+        assert gui.state.action_status is ActionStatus.EXECUTING
+        return accepted
+
+    execute.side_effect = dispatch
+    gui._submit_execute()
+    submissions[0]()
+
+    assert gui.state.plan_state == PanelPlanState()
+    assert gui.state.action_status is ActionStatus.IDLE
+    assert gui.state.last_result == f"execute={accepted}"
+
+
+def test_execute_exception_leaves_plan_consumed(executable_gui):
+    gui, submissions, execute = executable_gui
+    execute.side_effect = RuntimeError("dispatch failed")
+    gui._submit_execute()
+
+    with pytest.raises(RuntimeError, match="dispatch failed"):
+        submissions[0]()
+
+    assert gui.state.plan_state == PanelPlanState()
+
+
+def test_execute_validation_failure_retains_plan(executable_gui):
+    gui, submissions, execute = executable_gui
+    plan = gui.state.plan_state.plan
+    gui.state.target_status = TargetStatus.INFEASIBLE
+
+    gui._submit_execute()
+
+    assert gui.state.plan_state.plan is plan
+    assert submissions == []
+    execute.assert_not_called()
+
+
+def test_late_execute_result_preserves_newer_plan_after_cancel(executable_gui, mocker):
+    gui, submissions, execute = executable_gui
+    newer_plan = PanelPlanState(status=PlanStatus.FRESH)
+    mocker.patch.object(gui, "_restart_operation_worker")
+
+    def dispatch(_plan):
+        gui._submit_cancel()
+        assert gui.state.plan_state == PanelPlanState()
+        gui.state.plan_state = newer_plan
+        return False
+
+    execute.side_effect = dispatch
+    gui._submit_execute()
+    submissions[0]()
+
+    assert gui.state.plan_state is newer_plan
+    assert gui.state.plan_state.status is PlanStatus.FRESH
+    assert gui.state.last_result == "cancel=True"
+
+
+def test_gui_completion_enables_next_plan_without_cancel(executable_gui, module_factory, mocker):
+    gui, submissions, _execute = executable_gui
+    module = module_factory()
+    mocker.patch.object(gui, "operator", ManipulationOperator(module, mocker.Mock()))
+    status = mocker.patch.object(
+        module._control_coordinator,
+        "task_invoke",
+        return_value=TrajectoryStatus(state=TrajectoryState.EXECUTING),
+    )
+    cancel = mocker.spy(module, "cancel")
+
+    gui._submit_execute()
+    submissions[0]()
+    gui._refresh_model_state()
+    assert gui.state.manipulation_state == "EXECUTING"
+    assert gui.state.can_plan() is False
+    assert gui.state.can_cancel() is True
+    assert gui.state.plan_state == PanelPlanState()
+
+    status.return_value = TrajectoryStatus(state=TrajectoryState.COMPLETED)
+    gui._refresh_model_state()
+
+    assert gui.state.manipulation_state == "COMPLETED"
+    assert gui.state.can_plan() is True
+    assert gui.state.can_cancel() is False
+    assert gui.state.can_execute() is False
+    cancel.assert_not_called()
+
+
 @pytest.mark.parametrize(
     ("result", "success", "collision_free", "expected"),
     [