From ca4153dd5a48086c8e09284a55dd46ad38667ea5 Mon Sep 17 00:00:00 2001
From: Sebastian Castro <sebas.a.castro@gmail.com>
Date: Wed, 16 Oct 2024 21:01:32 -0400
Subject: [PATCH 1/3] Add simple BT.CPP solutions to Problems 2 and 3

---
 technologies/BehaviorTree.CPP/README.md       |  2 +-
 .../pyrobosim_btcpp/trees/problem2.xml        | 38 +++++++++++----
 .../pyrobosim_btcpp/trees/problem3.xml        | 48 +++++++++++++++++++
 3 files changed, 77 insertions(+), 11 deletions(-)
 create mode 100644 technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem3.xml

diff --git a/technologies/BehaviorTree.CPP/README.md b/technologies/BehaviorTree.CPP/README.md
index b98bea1..a59ee7d 100644
--- a/technologies/BehaviorTree.CPP/README.md
+++ b/technologies/BehaviorTree.CPP/README.md
@@ -34,7 +34,7 @@ For instance, consider this sample XML in [trees/navigation_demo.xml](pyrobosim_
 
 NOTE: remember that the `name` attribute in the XML is optional and used for debugging only.
 
-You can run the BeahviorTree with the command:
+You can run the BehaviorTree with the command:
 
 ```bash
 ros2 run pyrobosim_btcpp btcpp_executor --ros-args -p tree:=trees/navigation_demo.xml
diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem2.xml b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem2.xml
index f12f1a9..704c057 100644
--- a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem2.xml
+++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem2.xml
@@ -2,16 +2,34 @@
 
   <BehaviorTree ID="MainTree">
     <Sequence>
-        <Fallback>
-            <Navigate name="ToTrash" target="dumpster"/>
-            <Sequence>
-                <Navigate name="ToTrash" target="hall_dining_trash"/>
-                <Open target="hall_dining_trash"/>
-                <Navigate name="ToTrash" target="dumpster"/>
-            </Sequence>
-        </Fallback>
-        <Open target="dumpster"/>
-        <Navigate name="ToTable" target="table"/>
+      <Sequence>
+          <Fallback>
+              <Navigate name="ToTrash" target="dumpster"/>
+              <Sequence>
+                  <Navigate name="ToTrashRoomDoor" target="hall_dining_trash"/>
+                  <Open target="hall_dining_trash"/>
+                  <Navigate name="ToTrash" target="dumpster"/>
+              </Sequence>
+          </Fallback>
+          <Open target="dumpster"/>
+      </Sequence>
+      <Sequence>
+        <Navigate name="ToDesk" target="desk"/>
+        <PickObject name="PickDeskWaste" object="waste0"/>
+        <Navigate name="ToTrash" target="dumpster"/>
+        <PlaceObject name="PlaceDeskWaste" object="waste0"/>
+      </Sequence>
+      <Sequence>
+        <Navigate name="ToBin" target="bin"/>
+        <PickObject name="PickBinWaste" object="waste1"/>
+        <Navigate name="ToTrash" target="dumpster"/>
+        <PlaceObject name="PlaceBinWaste" object="waste1"/>
+      </Sequence>
+      <Sequence>
+        <Navigate name="ToDining" target="dining"/>
+        <Navigate name="ToTrashRoomDoor" target="hall_dining_trash"/>
+        <Close target="hall_dining_trash"/>
+      </Sequence>
     </Sequence>
   </BehaviorTree>
 
diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem3.xml b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem3.xml
new file mode 100644
index 0000000..6181e37
--- /dev/null
+++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem3.xml
@@ -0,0 +1,48 @@
+<root BTCPP_format="4">
+
+  <BehaviorTree ID="MainTree">
+    <Sequence>
+      <RetryUntilSuccessful num_attempts="5">
+        <Sequence>
+          <Navigate name="ToPantry" target="pantry"/>
+          <Open name="OpenPantry" target="pantry"/>
+          <PickObject name="PickBread" object="bread0"/>
+        </Sequence>
+      </RetryUntilSuccessful>
+      <RetryUntilSuccessful num_attempts="5">
+        <Sequence>
+          <Navigate name="ToTable" target="table"/>
+          <PlaceObject name="PlaceBread" object="bread0"/>
+        </Sequence>
+      </RetryUntilSuccessful>
+      <RetryUntilSuccessful num_attempts="5">
+        <Sequence>
+          <Navigate name="ToPantry" target="pantry"/>
+          <Close name="ClosePantry" target="pantry"/>
+        </Sequence>
+      </RetryUntilSuccessful>
+
+      <RetryUntilSuccessful num_attempts="5">
+        <Sequence>
+          <Navigate name="ToFridge" target="fridge"/>
+          <Open name="OpenFridge" target="fridge"/>
+          <PickObject name="PickButter" object="butter0"/>
+        </Sequence>
+      </RetryUntilSuccessful>
+      <RetryUntilSuccessful num_attempts="5">
+        <Sequence>
+          <Navigate name="ToTable" target="table"/>
+          <PlaceObject name="PlaceButter" object="butter0"/>
+        </Sequence>
+      </RetryUntilSuccessful>
+      <RetryUntilSuccessful num_attempts="5">
+        <Sequence>
+          <Navigate name="ToFridge" target="fridge"/>
+          <Close name="CloseFridge" target="fridge"/>
+        </Sequence>
+      </RetryUntilSuccessful>
+
+    </Sequence>
+  </BehaviorTree>
+
+</root>

From 6809fcc204496901aedaf0d166c9fe2cc3a1c32d Mon Sep 17 00:00:00 2001
From: Sebastian Castro <sebas.a.castro@gmail.com>
Date: Wed, 16 Oct 2024 21:14:26 -0400
Subject: [PATCH 2/3] WIP problem 4

---
 .../pyrobosim_btcpp/trees/problem4.xml        | 28 +++++++++++++++++++
 1 file changed, 28 insertions(+)
 create mode 100644 technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem4.xml

diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem4.xml b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem4.xml
new file mode 100644
index 0000000..93b67d1
--- /dev/null
+++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem4.xml
@@ -0,0 +1,28 @@
+<root BTCPP_format="4">
+
+  <!-- WORK IN PROGRESS -->
+  <!-- NOTE: This segfaults the BT executor when the battery crosses the threshold. -->
+  <BehaviorTree ID="MainTree">
+    <ReactiveSequence>
+      <Fallback>
+        <Inverter>
+          <IsBatteryLow low_threshold="90"/>
+        </Inverter>
+        <Navigate name="ToCharger" target="charger"/>
+      </Fallback>
+      <RetryUntilSuccessful num_attempts="10">
+        <Sequence>
+          <Navigate name="ToDesk" target="desk"/>
+          <Navigate name="ToFridge" target="fridge"/>
+          <Navigate name="ToDesk" target="desk"/>
+          <Navigate name="ToFridge" target="fridge"/>
+          <Navigate name="ToDesk" target="desk"/>
+          <Navigate name="ToFridge" target="fridge"/>
+          <Navigate name="ToDesk" target="desk"/>
+          <Navigate name="ToFridge" target="fridge"/>
+        </Sequence>
+      </RetryUntilSuccessful>
+    </ReactiveSequence>
+  </BehaviorTree>
+
+</root>

From 34b82999041760595cfc2dfbe078eab9f462fbd7 Mon Sep 17 00:00:00 2001
From: Sebastian Castro <sebas.a.castro@gmail.com>
Date: Sat, 19 Oct 2024 16:22:04 -0400
Subject: [PATCH 3/3] Add Problem 4 solution

---
 technologies/BehaviorTree.CPP/README.md       |   2 +-
 .../nodes/get_current_location_node.hpp       |  41 ++++
 .../pyrobosim_btcpp/src/btcpp_executor.cpp    |   2 +
 .../pyrobosim_btcpp/trees/problem4.xml        |  28 ---
 .../trees/pyrobosim_btcpp_model.xml           |   3 +
 .../trees/{ => solution}/problem1.xml         |   0
 .../trees/{ => solution}/problem2.xml         |   0
 .../trees/{ => solution}/problem3.xml         |  12 +-
 .../trees/solution/problem4.xml               | 175 ++++++++++++++++++
 9 files changed, 228 insertions(+), 35 deletions(-)
 delete mode 100644 technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem4.xml
 rename technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/{ => solution}/problem1.xml (100%)
 rename technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/{ => solution}/problem2.xml (100%)
 rename technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/{ => solution}/problem3.xml (82%)
 create mode 100644 technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/solution/problem4.xml

diff --git a/technologies/BehaviorTree.CPP/README.md b/technologies/BehaviorTree.CPP/README.md
index a59ee7d..a50b6b5 100644
--- a/technologies/BehaviorTree.CPP/README.md
+++ b/technologies/BehaviorTree.CPP/README.md
@@ -9,7 +9,7 @@ We suggest creating your XML trees in the folder `technologies/BehaviorTree.CPP/
 **Careful**: make sure to run this command in `/delib_ws` folder.
 
 ```bash
-colcon build --symlink-install --packages-select pyrobosim_btcpp
+colcon build --symlink-install --packages-up-to pyrobosim_btcpp
 ```
 
 ## How to run
diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/get_current_location_node.hpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/get_current_location_node.hpp
index e69de29..712680d 100644
--- a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/get_current_location_node.hpp
+++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/get_current_location_node.hpp
@@ -0,0 +1,41 @@
+
+#pragma once
+
+#include <behaviortree_cpp/action_node.h>
+#include <rclcpp/rclcpp.hpp>
+
+namespace BT
+{
+
+class GetCurrentLocation : public SyncActionNode
+{
+public:
+  GetCurrentLocation(const std::string& name, const NodeConfig& config, rclcpp::Logger logger)
+    : SyncActionNode(name, config), logger_(logger)
+  {}
+
+  static PortsList providedPorts()
+  {
+    return { OutputPort<std::string>("location", "The robot's current location.") };
+  }
+
+  // You must override the virtual function tick()
+  NodeStatus tick() override
+  {
+    // Read a value from the blackboard
+    std::string current_location = "";
+    if(!config().blackboard->get("@last_visited_location", current_location))
+    {
+      RCLCPP_ERROR(logger_, "Robot location not available yet");
+      return NodeStatus::FAILURE;
+    }
+    RCLCPP_INFO(logger_, "Robot location level: %s", current_location.c_str());
+    setOutput("location", current_location);
+    return NodeStatus::SUCCESS;
+  }
+
+private:
+  rclcpp::Logger logger_;
+};
+
+}  // namespace BT
diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/src/btcpp_executor.cpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/src/btcpp_executor.cpp
index 4bdeab0..30be81d 100644
--- a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/src/btcpp_executor.cpp
+++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/src/btcpp_executor.cpp
@@ -13,6 +13,7 @@
 
 // BTCPP nodes in this package
 #include "pyrobosim_btcpp/nodes/battery_nodes.hpp"
+#include "pyrobosim_btcpp/nodes/get_current_location_node.hpp"
 #include "pyrobosim_btcpp/nodes/open_node.hpp"
 #include "pyrobosim_btcpp/nodes/close_node.hpp"
 #include "pyrobosim_btcpp/nodes/detect_object_node.hpp"
@@ -72,6 +73,7 @@ int main(int argc, char** argv)
 
   factory.registerNodeType<BT::IsBatteryLow>("IsBatteryLow", nh->get_logger());
   factory.registerNodeType<BT::IsBatteryFull>("IsBatteryFull", nh->get_logger());
+  factory.registerNodeType<BT::GetCurrentLocation>("GetCurrentLocation", nh->get_logger());
   factory.registerNodeType<BT::CloseAction>("Close", params);
   factory.registerNodeType<BT::DetectObject>("DetectObject", params);
   factory.registerNodeType<BT::NavigateAction>("Navigate", params);
diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem4.xml b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem4.xml
deleted file mode 100644
index 93b67d1..0000000
--- a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem4.xml
+++ /dev/null
@@ -1,28 +0,0 @@
-<root BTCPP_format="4">
-
-  <!-- WORK IN PROGRESS -->
-  <!-- NOTE: This segfaults the BT executor when the battery crosses the threshold. -->
-  <BehaviorTree ID="MainTree">
-    <ReactiveSequence>
-      <Fallback>
-        <Inverter>
-          <IsBatteryLow low_threshold="90"/>
-        </Inverter>
-        <Navigate name="ToCharger" target="charger"/>
-      </Fallback>
-      <RetryUntilSuccessful num_attempts="10">
-        <Sequence>
-          <Navigate name="ToDesk" target="desk"/>
-          <Navigate name="ToFridge" target="fridge"/>
-          <Navigate name="ToDesk" target="desk"/>
-          <Navigate name="ToFridge" target="fridge"/>
-          <Navigate name="ToDesk" target="desk"/>
-          <Navigate name="ToFridge" target="fridge"/>
-          <Navigate name="ToDesk" target="desk"/>
-          <Navigate name="ToFridge" target="fridge"/>
-        </Sequence>
-      </RetryUntilSuccessful>
-    </ReactiveSequence>
-  </BehaviorTree>
-
-</root>
diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/pyrobosim_btcpp_model.xml b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/pyrobosim_btcpp_model.xml
index a33a602..ffd45ab 100644
--- a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/pyrobosim_btcpp_model.xml
+++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/pyrobosim_btcpp_model.xml
@@ -16,6 +16,9 @@
         <Condition ID="IsBatteryLow">
             <input_port name="low_threshold" type="double" default="20.000000">Return SUCCESS if {@battery_level} below this value</input_port>
         </Condition>
+        <Action ID="GetCurrentLocation"/>
+            <output_port name="location" type="std::string"/>
+        </Action>
         <Action ID="Navigate">
             <input_port name="target" type="std::string"/>
             <input_port name="robotID" type="std::string" default="robot">Robot name</input_port>
diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem1.xml b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/solution/problem1.xml
similarity index 100%
rename from technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem1.xml
rename to technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/solution/problem1.xml
diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem2.xml b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/solution/problem2.xml
similarity index 100%
rename from technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem2.xml
rename to technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/solution/problem2.xml
diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem3.xml b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/solution/problem3.xml
similarity index 82%
rename from technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem3.xml
rename to technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/solution/problem3.xml
index 6181e37..453282a 100644
--- a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem3.xml
+++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/solution/problem3.xml
@@ -2,40 +2,40 @@
 
   <BehaviorTree ID="MainTree">
     <Sequence>
-      <RetryUntilSuccessful num_attempts="5">
+      <RetryUntilSuccessful num_attempts="10">
         <Sequence>
           <Navigate name="ToPantry" target="pantry"/>
           <Open name="OpenPantry" target="pantry"/>
           <PickObject name="PickBread" object="bread0"/>
         </Sequence>
       </RetryUntilSuccessful>
-      <RetryUntilSuccessful num_attempts="5">
+      <RetryUntilSuccessful num_attempts="10">
         <Sequence>
           <Navigate name="ToTable" target="table"/>
           <PlaceObject name="PlaceBread" object="bread0"/>
         </Sequence>
       </RetryUntilSuccessful>
-      <RetryUntilSuccessful num_attempts="5">
+      <RetryUntilSuccessful num_attempts="10">
         <Sequence>
           <Navigate name="ToPantry" target="pantry"/>
           <Close name="ClosePantry" target="pantry"/>
         </Sequence>
       </RetryUntilSuccessful>
 
-      <RetryUntilSuccessful num_attempts="5">
+      <RetryUntilSuccessful num_attempts="10">
         <Sequence>
           <Navigate name="ToFridge" target="fridge"/>
           <Open name="OpenFridge" target="fridge"/>
           <PickObject name="PickButter" object="butter0"/>
         </Sequence>
       </RetryUntilSuccessful>
-      <RetryUntilSuccessful num_attempts="5">
+      <RetryUntilSuccessful num_attempts="10">
         <Sequence>
           <Navigate name="ToTable" target="table"/>
           <PlaceObject name="PlaceButter" object="butter0"/>
         </Sequence>
       </RetryUntilSuccessful>
-      <RetryUntilSuccessful num_attempts="5">
+      <RetryUntilSuccessful num_attempts="10">
         <Sequence>
           <Navigate name="ToFridge" target="fridge"/>
           <Close name="CloseFridge" target="fridge"/>
diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/solution/problem4.xml b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/solution/problem4.xml
new file mode 100644
index 0000000..a19d553
--- /dev/null
+++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/solution/problem4.xml
@@ -0,0 +1,175 @@
+<root BTCPP_format="4">
+
+  <!-- The main tree, which uses the other subtrees. -->
+  <BehaviorTree ID="MainTree">
+    <Sequence>
+      <SubTree ID="OpenDoorToCharger"/>
+      <SubTree ID="PrepareMeal"/>
+      <SubTree ID="TakeOutTrash"/>
+    </Sequence>
+  </BehaviorTree>
+
+  <!-- Open the door to the closet, which contains the charger. -->
+  <BehaviorTree ID="OpenDoorToCharger">
+    <Sequence>
+      <RetryUntilSuccessful num_attempts="10">
+        <Navigate name="ToClosetDoor" target="hall_dining_closet"/>
+      </RetryUntilSuccessful>
+      <RetryUntilSuccessful num_attempts="10">
+        <Open name="OpenClosetDoor" target="hall_dining_closet"/>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <!-- Bring the bread and butter to the table, paying attention to battery. -->
+  <BehaviorTree ID="PrepareMeal">
+    <Sequence>
+      <SubTree ID="NavigateRobustly" target="pantry"/>
+      <SubTree ID="OpenRobustly" target="pantry"/>
+      <SubTree ID="PickRobustly" object="bread0"/>
+      <SubTree ID="NavigateRobustly" target="table"/>
+      <SubTree ID="PlaceRobustly" object="bread0"/>
+      <SubTree ID="NavigateRobustly" target="pantry"/>
+      <SubTree ID="CloseRobustly" target="pantry"/>
+      <SubTree ID="NavigateRobustly" target="fridge"/>
+      <SubTree ID="OpenRobustly" target="fridge"/>
+      <SubTree ID="PickRobustly" object="butter0"/>
+      <SubTree ID="NavigateRobustly" target="table"/>
+      <SubTree ID="PlaceRobustly" object="butter0"/>
+      <SubTree ID="NavigateRobustly" target="fridge"/>
+      <SubTree ID="CloseRobustly" target="fridge"/>
+    </Sequence>
+  </BehaviorTree>
+
+
+  <!-- Take the trash to the dumpster, paying attention to battery. -->
+  <BehaviorTree ID="TakeOutTrash">
+    <Sequence>
+      <SubTree ID="NavigateRobustly" target="hall_dining_trash"/>
+      <SubTree ID="OpenRobustly" target="hall_dining_trash"/>
+      <SubTree ID="NavigateRobustly" target="dumpster"/>
+      <SubTree ID="OpenRobustly" target="dumpster"/>
+      <SubTree ID="NavigateRobustly" target="desk"/>
+      <SubTree ID="PickRobustly" object="waste0"/>
+      <SubTree ID="NavigateRobustly" target="dumpster"/>
+      <SubTree ID="PlaceRobustly" object="waste0"/>
+      <SubTree ID="NavigateRobustly" target="bin"/>
+      <SubTree ID="PickRobustly" object="waste1"/>
+      <SubTree ID="NavigateRobustly" target="dumpster"/>
+      <SubTree ID="PlaceRobustly" object="waste1"/>
+      <SubTree ID="CloseRobustly" target="dumpster"/>
+      <SubTree ID="NavigateRobustly" target="dining"/>
+      <SubTree ID="NavigateRobustly" target="hall_dining_trash"/>
+      <SubTree ID="CloseRobustly" target="hall_dining_trash"/>
+    </Sequence>
+  </BehaviorTree>
+
+  <!-- Helper subtrees that perform basic actions,
+  paying attention to both failures and battery usage. -->
+  <BehaviorTree ID="NavigateRobustly">
+    <RetryUntilSuccessful num_attempts="5">
+      <Fallback>
+        <ReactiveSequence>
+          <IsBatteryFull full_threshold="30"/>
+          <RetryUntilSuccessful num_attempts="10">
+            <Navigate name="GoToTarget" target="{target}"/>
+          </RetryUntilSuccessful>
+        </ReactiveSequence>
+        <AlwaysFailure>
+          <RetryUntilSuccessful num_attempts="10">
+            <Navigate name="GoToCharger" target="charger"/>
+          </RetryUntilSuccessful>
+        </AlwaysFailure>
+      </Fallback>
+    </RetryUntilSuccessful>
+  </BehaviorTree>
+
+  <BehaviorTree ID="PickRobustly">
+    <Sequence>
+      <GetCurrentLocation location="{start_location}"/>
+      <RetryUntilSuccessful num_attempts="10">
+        <Sequence>
+          <PickObject name="PickObject" object="{object}"/>
+          <Fallback>
+            <IsBatteryFull full_threshold="30"/>
+            <Sequence>
+              <RetryUntilSuccessful num_attempts="10">
+                <Navigate name="GoToCharger" target="charger"/>
+              </RetryUntilSuccessful>
+              <RetryUntilSuccessful num_attempts="10">
+                <Navigate name="GoBackToStart" target="{start_location}"/>
+              </RetryUntilSuccessful>
+            </Sequence>
+          </Fallback>
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="PlaceRobustly">
+    <Sequence>
+      <GetCurrentLocation location="{start_location}"/>
+      <RetryUntilSuccessful num_attempts="10">
+        <Sequence>
+          <PlaceObject name="PlaceObject" object="{object}"/>
+          <Fallback>
+            <IsBatteryFull full_threshold="30"/>
+            <Sequence>
+              <RetryUntilSuccessful num_attempts="10">
+                <Navigate name="GoToCharger" target="charger"/>
+              </RetryUntilSuccessful>
+              <RetryUntilSuccessful num_attempts="10">
+                <Navigate name="GoBackToStart" target="{start_location}"/>
+              </RetryUntilSuccessful>
+            </Sequence>
+          </Fallback>
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="OpenRobustly">
+    <Sequence>
+      <GetCurrentLocation location="{start_location}"/>
+      <RetryUntilSuccessful num_attempts="10">
+        <Sequence>
+          <Open name="OpenLocation" target="{target}"/>
+          <Fallback>
+            <IsBatteryFull full_threshold="30"/>
+            <Sequence>
+              <RetryUntilSuccessful num_attempts="10">
+                <Navigate name="GoToCharger" target="charger"/>
+              </RetryUntilSuccessful>
+              <RetryUntilSuccessful num_attempts="10">
+                <Navigate name="GoBackToStart" target="{start_location}"/>
+              </RetryUntilSuccessful>
+            </Sequence>
+          </Fallback>
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="CloseRobustly">
+    <Sequence>
+      <GetCurrentLocation location="{start_location}"/>
+      <RetryUntilSuccessful num_attempts="10">
+        <Sequence>
+          <Close name="CloseLocation" target="{target}"/>
+          <Fallback>
+            <IsBatteryFull full_threshold="30"/>
+            <Sequence>
+              <RetryUntilSuccessful num_attempts="10">
+                <Navigate name="GoToCharger" target="charger"/>
+              </RetryUntilSuccessful>
+              <RetryUntilSuccessful num_attempts="10">
+                <Navigate name="GoBackToStart" target="{start_location}"/>
+              </RetryUntilSuccessful>
+            </Sequence>
+          </Fallback>
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+</root>