Remove main behavior tree

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
osrf · Dec 31, 2024 · 65b7704 · 65b7704
1 parent a61c71e
commit 65b7704
Show file tree

Hide file tree

Showing 7 changed files with 31 additions and 204 deletions.
diff --git a/nexus_integration_tests/config/system_bts/main.xml b/nexus_integration_tests/config/system_bts/main.xml
diff --git a/nexus_integration_tests/config/system_bts/pick_and_place.xml b/nexus_integration_tests/config/system_bts/pick_and_place.xml
@@ -2,14 +2,30 @@
   <BehaviorTree ID="pick_and_place">
     <ReactiveSequence>
       <IsPauseTriggered paused="{paused}"/>
-      <Parallel success_threshold="-1">
-        <Sequence>
-          <BidTransporter name="bid_transporterasdasd" destination="{workcell}" result="{transporter}"/>
-          <TransporterRequest name="transporter_request" transporter="{transporter}" destination="{workcell}"/>
-          <SendSignal task="{task}" signal="transporter_done"/>
-        </Sequence>
-        <WorkcellRequest name="workcell_request" workcell="{workcell}" task="{task}"/>
-      </Parallel>
+      <Fallback>
+        <PausableSequence pause="{paused}">
+          <ForEachTask task="{task}" workcell="{workcell}">
+            <ReactiveSequence>
+              <IsPauseTriggered paused="{paused}"/>
+              <Parallel success_threshold="-1">
+                <Sequence>
+                  <BidTransporter name="bid_transporterasdasd" destination="{workcell}" result="{transporter}"/>
+                  <TransporterRequest name="transporter_request" transporter="{transporter}" destination="{workcell}"/>
+                  <SendSignal task="{task}" signal="transporter_done"/>
+                </Sequence>
+                <WorkcellRequest name="workcell_request" workcell="{workcell}" task="{task}"/>
+              </Parallel>
+            </ReactiveSequence>
+          </ForEachTask>
+          <BidTransporter name="bid_transporter" destination="unloading" result="{transporter}"/>
+          <TransporterRequest name="transporter_request" transporter="{transporter}" destination="unloading"/>
+        </PausableSequence>
+        <PausableSequence pause="{paused}">
+          <BidTransporter name="bid_transporter" destination="unloading" result="{transporter}"/>
+          <TransporterRequest name="transporter_request" transporter="{transporter}" destination="unloading"/>
+          <AlwaysFailure/>
+        </PausableSequence>
+      </Fallback>
     </ReactiveSequence>
   </BehaviorTree>
 </root>
diff --git a/nexus_integration_tests/launch/control_center.launch.py b/nexus_integration_tests/launch/control_center.launch.py
@@ -126,7 +126,7 @@ def launch_setup(context, *args, **kwargs):
             {
                 "bt_path": (
                     FindPackageShare("nexus_integration_tests"),
-                    "/config/system_bts",
+                    "/config/system_bts/pick_and_place.xml",
                 ),
                 "remap_task_types":
                     """{

diff --git a/nexus_system_orchestrator/CMakeLists.txt b/nexus_system_orchestrator/CMakeLists.txt
@@ -19,7 +19,6 @@ set(CMAKE_CXX_FLAGS "-Wall -Wpedantic")
 
 add_library(${PROJECT_NAME}_plugin SHARED
   src/bid_transporter.cpp
-  src/execute_task.cpp
   src/for_each_task.cpp
   src/send_signal.cpp
   src/system_orchestrator.cpp

diff --git a/nexus_system_orchestrator/src/execute_task.cpp b/nexus_system_orchestrator/src/execute_task.cpp
diff --git a/nexus_system_orchestrator/src/execute_task.hpp b/nexus_system_orchestrator/src/execute_task.hpp
diff --git a/nexus_system_orchestrator/src/system_orchestrator.cpp b/nexus_system_orchestrator/src/system_orchestrator.cpp
@@ -20,7 +20,6 @@
 #include "bid_transporter.hpp"
 #include "context.hpp"
 #include "exceptions.hpp"
-#include "execute_task.hpp"
 #include "for_each_task.hpp"
 #include "job.hpp"
 #include "send_signal.hpp"
@@ -71,20 +70,19 @@ SystemOrchestrator::SystemOrchestrator(const rclcpp::NodeOptions& options)
     ParameterDescriptor desc;
     desc.read_only = true;
     desc.description =
-      "Path to a directory containing behavior trees. Each file in the directory should be a behavior tree xml, the file name denotes the task type for that behavior tree. In addition, there must be a file named \"main.xml\" which will be used to perform the work order.";
+      "Path to a behavior tree.";
     this->_bt_path = this->declare_parameter("bt_path", "", desc);
     if (this->_bt_path.empty())
     {
       throw std::runtime_error("param [bt_path] is required");
     }
 
-    // check if "main.xml" exists
-    const auto main_bt = this->_bt_path / "main.xml";
-    if (!std::filesystem::exists(main_bt) ||
-      !std::filesystem::is_regular_file(main_bt))
+    // check if behavior tree exists
+    if (!std::filesystem::exists(this->_bt_path) ||
+      !std::filesystem::is_regular_file(this->_bt_path))
     {
       throw std::runtime_error(
-              "path specified in [bt_path] param must contain \"main.xml\"");
+              "path specified in [bt_path] param must point to a file");
     }
   }
 
@@ -491,21 +489,13 @@ BT::Tree SystemOrchestrator::_create_bt(const WorkOrderActionType::Goal& wo,
       this->get_logger(), ctx);
     });
 
-  bt_factory->registerBuilder<ExecuteTask>("ExecuteTask",
-    [this, ctx, bt_factory](const std::string& name,
-    const BT::NodeConfiguration& config)
-    {
-      return std::make_unique<ExecuteTask>(name, config, ctx, this->_bt_path,
-      bt_factory);
-    });
-
   bt_factory->registerBuilder<SendSignal>("SendSignal",
     [ctx](const std::string& name, const BT::NodeConfiguration& config)
     {
       return std::make_unique<SendSignal>(name, config, ctx);
     });
 
-  return bt_factory->createTreeFromFile(this->_bt_path / "main.xml");
+  return bt_factory->createTreeFromFile(this->_bt_path);
 }
 
 void SystemOrchestrator::_create_job(const WorkOrderActionType::Goal& goal)