BT.CPP: refine solution for prblems 1 and 2

ros-wg-delib · Oct 20, 2024 · d6b8531 · d6b8531
1 parent f52a109
commit d6b8531
Show file tree

Hide file tree

Showing 4 changed files with 28 additions and 40 deletions.
diff --git a/technologies/BehaviorTree.CPP/README.md b/technologies/BehaviorTree.CPP/README.md
@@ -48,14 +48,15 @@ The argument `tree` above is the path to the XML file. The path can be either:
 
 ## Action Nodes
 
-| Action Name  | Description                                 | Input Port                                                                     |
-|--------------|---------------------------------------------|--------------------------------------------------------------------------------|
-| Close        | Close a door or a container                 | - location: name of the location                                               |
-| DetectObject | Return SUCCESS if the object is detected    | - object: name of the object                                                   |
-| Navigate     | Move to a specific location                 | - target: name of the location                                                 |
-| Open         | Open a door or a container                  | - object: name of the object                                                   |
-| PickObject   | Pick an object (you must be in front of it) | - object: name of the object <br>- location: (optional) where the object is        |
-| PlaceObject  | Place an object (you must hold it)          | - object: name of the object <br>- location: (optional) where the object should go |
+| Action Name        | Description                                               | Input Port                            |
+|--------------------|-----------------------------------------------------------|---------------------------------------|
+| Close              | Close a door or a container                               | - location: name of the location      |
+| DetectObject       | Return SUCCESS if the object is detected                  | - object: name of the object          |
+| GetCurrentLocation | Store current location in the output port "location".     |                                       |
+| Navigate           | Move to a specific location                               | - target: name of the location        |
+| Open               | Open a door or a container                                | - object: name of the object          |
+| PickObject         | Pick an object (you must be in front of it)               | - object: name of the object <br>- location: (optional) where the object is        |
+| PlaceObject        | Place an object (you must hold it)                        | - object: name of the object <br>- location: (optional) where the object should go |
 
 Note:
 

diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/src/btcpp_executor.cpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/src/btcpp_executor.cpp
@@ -70,6 +70,7 @@ int main(int argc, char** argv)
   BT::RosNodeParams params;
   params.nh = nh;
   params.default_port_value = "execute_action";
+  params.wait_for_server_timeout = std::chrono::seconds(5);
 
   factory.registerNodeType<BT::IsBatteryLow>("IsBatteryLow", nh->get_logger());
   factory.registerNodeType<BT::IsBatteryFull>("IsBatteryFull", nh->get_logger());

diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/solution/problem1.xml b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/solution/problem1.xml
@@ -12,12 +12,6 @@
             </Sequence>
             <Navigate name="ToTableWithoutSnack" target="table"/>
         </IfThenElse>
-        <Sequence name="DetectSnackBackToPantry">
-            <PickObject name="PickSnack" object="snacks0"/>
-            <Navigate name="ToPantry" target="pantry"/>
-            <PlaceObject name="PlaceSnack" object="snacks0"/>
-            <Navigate name="ToTable" target="table"/>
-        </Sequence>
     </Sequence>
   </BehaviorTree>
 

diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/solution/problem2.xml b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/solution/problem2.xml
@@ -2,34 +2,26 @@
 
   <BehaviorTree ID="MainTree">
     <Sequence>
-      <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"/>
+      <Fallback>
         <Navigate name="ToTrash" target="dumpster"/>
-        <PlaceObject name="PlaceDeskWaste" object="waste0"/>
-      </Sequence>
-      <Sequence>
-        <Navigate name="ToBin" target="bin"/>
-        <PickObject name="PickBinWaste" object="waste1"/>
+        <Sequence>
+          <Navigate name="ToTrashRoomDoor" target="hall_dining_trash"/>
+          <Open target="hall_dining_trash"/>
+          <Navigate name="ToTrash" target="dumpster"/>
+        </Sequence>
+      </Fallback>
+      <Open target="dumpster"/>
+      <SubTree ID="DisposeWaste" waste_location="desk" waste="waste0"/>
+      <SubTree ID="DisposeWaste" waste_location="bin" waste="waste1"/>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="DisposeWaste">
+    <Sequence>
+        <Navigate name="ToDesk" target="{waste_location}"/>
+        <PickObject name="PickDeskWaste" object="{waste}"/>
         <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>
+        <PlaceObject name="PlaceDeskWaste" object="{waste}"/>
     </Sequence>
   </BehaviorTree>