diff --git a/nexus_integration_tests/launch/control_center.launch.py b/nexus_integration_tests/launch/control_center.launch.py index c92b133..e9f62d2 100644 --- a/nexus_integration_tests/launch/control_center.launch.py +++ b/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), ] ) diff --git a/nexus_system_orchestrator/src/system_orchestrator.cpp b/nexus_system_orchestrator/src/system_orchestrator.cpp index bd972e2..9fbbaa8 100644 --- a/nexus_system_orchestrator/src/system_orchestrator.cpp +++ b/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>( // this->get_name(), std::vector{}); }); + + this->_param_cb_handle = this->add_on_set_parameters_callback( + [this](const std::vector& 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())) + { + 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(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&)> on_done) { diff --git a/nexus_system_orchestrator/src/system_orchestrator.hpp b/nexus_system_orchestrator/src/system_orchestrator.hpp index 0cbc8a3..51ac5b5 100644 --- a/nexus_system_orchestrator/src/system_orchestrator.hpp +++ b/nexus_system_orchestrator/src/system_orchestrator.hpp @@ -85,6 +85,7 @@ class SystemOrchestrator : public rclcpp::Service::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> _task_remaps; + std::shared_ptr _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.