From 8daf2e2c8cdb59db8c8528d8696be3aa6d60c387 Mon Sep 17 00:00:00 2001
From: Mario Prats <mario.prats@picknik.ai>
Date: Fri, 14 Nov 2025 14:37:27 +0100
Subject: [PATCH] clear octomap at the end of octomap example

---
 src/lab_sim/objectives/octomap_example.xml      | 11 +++++++++++
 src/lab_sim/test/objectives_integration_test.py |  1 +
 2 files changed, 12 insertions(+)

diff --git a/src/lab_sim/objectives/octomap_example.xml b/src/lab_sim/objectives/octomap_example.xml
index a7a50f30..4b3f9ee6 100644
--- a/src/lab_sim/objectives/octomap_example.xml
+++ b/src/lab_sim/objectives/octomap_example.xml
@@ -25,6 +25,17 @@
         pcd_topic="/pcd_pointcloud_captures"
         point_cloud="{point_cloud}"
       />
+      <Action
+        ID="LogMessage"
+        log_level="info"
+        message="Click Resume to reset the octomap and finish the Behavior"
+      />
+      <!--Clear the Octomap at the end to avoid interfering with other Behaviors-->
+      <Action
+        ID="BreakpointSubscriber"
+        breakpoint_topic="/moveit_pro_breakpoint"
+      />
+      <SubTree ID="Clear Snapshot" _collapsed="true" />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
diff --git a/src/lab_sim/test/objectives_integration_test.py b/src/lab_sim/test/objectives_integration_test.py
index d12e0bff..b7cc4827 100644
--- a/src/lab_sim/test/objectives_integration_test.py
+++ b/src/lab_sim/test/objectives_integration_test.py
@@ -77,6 +77,7 @@
     "Pick 1 Pill Bottle",
     "AddBottlesToPlanningScene",
     "Interactive Marker Visualization",  # Server not available for GetTextFromUser
+    "Octomap Example",  # Requires user input to clear the octomap.
 }