Make main behavior tree configurable

nexus_integration_tests/launch/control_center.launch.py

@@ -112,6 +112,7 @@ def launch_setup(context, *args, **kwargs):
     transporter_plugin = LaunchConfiguration("transporter_plugin")
     activate_system_orchestrator = LaunchConfiguration("activate_system_orchestrator")
     headless = LaunchConfiguration("headless")
+    main_bt_filename = LaunchConfiguration("main_bt_filename")
 
     nexus_panel_rviz_path = os.path.join(
         get_package_share_directory("nexus_integration_tests"), "rviz", "nexus_panel.rviz"
@@ -132,6 +133,7 @@ def launch_setup(context, *args, **kwargs):
                     """{
                         pick_and_place: [place_on_conveyor, pick_from_conveyor],
                     }""",
+                "main_bt_filename": main_bt_filename,
                 "max_jobs": 2,
             }
         ],
@@ -243,6 +245,11 @@ def generate_launch_description():
                 default_value="true",
                 description="Launch in headless mode (no gui)",
             ),
+            DeclareLaunchArgument(
+                "main_bt_filename",
+                default_value="main.xml",
+                description="File name of the main system orchestrator behavior tree",
+            ),
             OpaqueFunction(function=launch_setup),
         ]
     )

nexus_system_orchestrator/src/system_orchestrator.cpp

@@ -71,20 +71,31 @@ 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 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.";
     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))
+    if (!std::filesystem::exists(this->_bt_path) ||
+      !std::filesystem::is_directory(this->_bt_path))
     {
       throw std::runtime_error(
-              "path specified in [bt_path] param must contain \"main.xml\"");
+              "path specified in [bt_path] param must be a folder");
+    }
+  }
+
+  {
+    ParameterDescriptor desc;
+    desc.description =
+      "Filename of the main behavior tree to run. Paths will be resolved relative to the \"bt_path\" parameter. Defaults to \"main.xml\".";
+    this->_main_bt_filename = this->declare_parameter("main_bt_filename", "main.xml", desc);
+
+    if (!this->_bt_filename_valid(this->_main_bt_filename))
+    {
+      throw std::runtime_error(
+              "[bt_path] and [main_bt_filename] don't point to a file");
     }
   }
 
@@ -139,6 +150,24 @@ SystemOrchestrator::SystemOrchestrator(const rclcpp::NodeOptions& options)
         // this->_lifecycle_mgr = std::make_unique<lifecycle_manager::LifecycleManager<>>(
         //   this->get_name(), std::vector<std::string>{});
       });
+
+  this->_param_cb_handle = this->add_on_set_parameters_callback(
+      [this](const std::vector<rclcpp::Parameter>& parameters)
+      {
+        rcl_interfaces::msg::SetParametersResult result;
+        result.successful = true;
+        for (const auto& parameter: parameters)
+        {
+          if (parameter.get_name() == "main_bt_filename" && !this->_bt_filename_valid(parameter.get_value<std::string>()))
+          {
+            result.reason = "main_bt_filename points to a non existing file";
+            result.successful = false;
+            break;
+          }
+        }
+        return result;
+      });
+
 }
 
 auto SystemOrchestrator::on_configure(const rclcpp_lifecycle::State& previous)
@@ -505,7 +534,7 @@ BT::Tree SystemOrchestrator::_create_bt(const WorkOrderActionType::Goal& wo,
       return std::make_unique<SendSignal>(name, config, ctx);
     });
 
-  return bt_factory->createTreeFromFile(this->_bt_path / "main.xml");
+  return bt_factory->createTreeFromFile(this->_bt_path / this->_main_bt_filename);
 }
 
 void SystemOrchestrator::_create_job(const WorkOrderActionType::Goal& goal)
@@ -954,6 +983,17 @@ void SystemOrchestrator::_spin_bts_once()
   }
 }
 
+bool SystemOrchestrator::_bt_filename_valid(const std::string& bt_filename) const
+{
+  const auto resolved_bt = this->_bt_path / bt_filename;
+  if (!std::filesystem::exists(resolved_bt) ||
+    !std::filesystem::is_regular_file(resolved_bt))
+  {
+    return false;
+  }
+  return true;
+}
+
 void SystemOrchestrator::_assign_workcell_task(const WorkcellTask& task,
   std::function<void(const std::optional<std::string>&)> on_done)
 {

nexus_system_orchestrator/src/system_orchestrator.hpp

@@ -85,6 +85,7 @@ class SystemOrchestrator : public
   rclcpp::Service<endpoints::GetWorkOrderStateService::ServiceType>::SharedPtr
     _get_wo_state_srv;
   std::filesystem::path _bt_path;
+  std::string _main_bt_filename;
   rclcpp::SubscriptionBase::SharedPtr _cancel_wo_sub;
   bool _activated = false;
   bool _estop_pressed = false;
@@ -97,6 +98,7 @@ class SystemOrchestrator : public
   rclcpp::SubscriptionBase::SharedPtr _estop_sub;
   // mapping of mapped task type and the original
   std::shared_ptr<std::unordered_map<std::string, std::string>> _task_remaps;
+  std::shared_ptr<OnSetParametersCallbackHandle> _param_cb_handle;
 
   /**
    * Creates a BT from a work order.
@@ -160,6 +162,11 @@ class SystemOrchestrator : public
 
   std::string _workcell_namespace(const std::string& workcell_id);
 
+  /**
+   * Checks if the requested filename points to a proper file
+   */
+  bool _bt_filename_valid(const std::string& bt_filename) const;
+
   /**
    * Send bid requests and assign the task to the most suitable workcell.
    * Currently this always assign to the first workcell that accepts the bid.