Add parameter for bt filename and parameter callback

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

nexus_system_orchestrator/src/system_orchestrator.cpp

@@ -70,19 +70,36 @@ SystemOrchestrator::SystemOrchestrator(const rclcpp::NodeOptions& options)
     ParameterDescriptor desc;
     desc.read_only = true;
     desc.description =
-      "Path to a behavior tree.";
+      "Path to a directory containing behavior trees. Each file in the directory should be a behavior tree xml.";
     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 behavior tree exists
     if (!std::filesystem::exists(this->_bt_path) ||
-      !std::filesystem::is_regular_file(this->_bt_path))
+      !std::filesystem::is_directory(this->_bt_path))
     {
       throw std::runtime_error(
-              "path specified in [bt_path] param must point to a file");
+              "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->_bt_filename = this->declare_parameter("bt_filename", "main.xml", desc);
+    if (this->_bt_path.empty())
+    {
+      throw std::runtime_error("param [bt_path] is required");
+    }
+
+    const auto resolved_bt = this->_bt_path / this->_bt_filename;
+    if (!this->_bt_filename_valid(this->_bt_filename))
+    {
+      throw std::runtime_error(
+              "[bt_path] and [bt_filename] don't point to a file");
     }
   }
 
@@ -117,6 +134,23 @@ 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() == "bt_filename" && !this->_bt_filename_valid(parameter.get_value<std::string>()))
+          {
+            result.reason = "bt_filename points to a non existing file";
+            result.successful = false;
+            break;
+          }
+        }
+        return result;
+      });
 }
 
 auto SystemOrchestrator::on_configure(const rclcpp_lifecycle::State& previous)
@@ -475,7 +509,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);
+  return bt_factory->createTreeFromFile(this->_bt_path / this->_bt_filename);
 }
 
 void SystemOrchestrator::_create_job(const WorkOrderActionType::Goal& goal)
@@ -924,6 +958,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 _bt_filename;
   rclcpp::SubscriptionBase::SharedPtr _cancel_wo_sub;
   bool _activated = false;
   bool _estop_pressed = false;
@@ -95,6 +96,7 @@ class SystemOrchestrator : public
   std::unique_ptr<lifecycle_manager::LifecycleManager<>> _lifecycle_mgr{nullptr};
   rclcpp::TimerBase::SharedPtr _pre_configure_timer;
   rclcpp::SubscriptionBase::SharedPtr _estop_sub;
+  std::shared_ptr<OnSetParametersCallbackHandle> _param_cb_handle;
 
   /**
    * Creates a BT from a work order.
@@ -158,6 +160,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.