From bb542fd4ee4215f3b9825a0cf3889d7e3decd50d Mon Sep 17 00:00:00 2001 From: "VM (Vicky) Brasseur" <298927+vmbrasseur@users.noreply.github.com> Date: Mon, 30 Dec 2024 16:48:18 -0800 Subject: [PATCH 1/3] Add Apache-2.0 license (#45) Per #44, this repository was lacking a license. This fixes that oversight. :-) --- LICENSE | 202 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 202 insertions(+) create mode 100644 LICENSE diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. From 12b91400d60337ff1d4debdffb4065a378e09700 Mon Sep 17 00:00:00 2001 From: yadunund Date: Tue, 31 Dec 2024 09:08:35 -0800 Subject: [PATCH 2/3] Load plugin to check if a task is doable (#46) Signed-off-by: Yadunund --- nexus_capabilities/CMakeLists.txt | 28 +++++++ .../nexus_capabilities/task_checker.hpp | 51 +++++++++++++ .../src/task_checkers/filepath_checker.cpp | 73 +++++++++++++++++++ .../src/task_checkers/plugins.xml | 5 ++ nexus_integration_tests/launch/launch.py | 2 + .../launch/workcell.launch.py | 6 ++ .../src/workcell_orchestrator.cpp | 54 +++++++++----- .../src/workcell_orchestrator.hpp | 9 +-- 8 files changed, 206 insertions(+), 22 deletions(-) create mode 100644 nexus_capabilities/include/nexus_capabilities/task_checker.hpp create mode 100644 nexus_capabilities/src/task_checkers/filepath_checker.cpp create mode 100644 nexus_capabilities/src/task_checkers/plugins.xml diff --git a/nexus_capabilities/CMakeLists.txt b/nexus_capabilities/CMakeLists.txt index 6f9d4a3..063bf86 100644 --- a/nexus_capabilities/CMakeLists.txt +++ b/nexus_capabilities/CMakeLists.txt @@ -150,4 +150,32 @@ install( pluginlib_export_plugin_description_file(nexus_capabilities src/capabilities/plugins.xml) +#=============================================================================== +# builtin task checkers +#=============================================================================== +add_library(nexus_builtin_task_checkers SHARED + src/task_checkers/filepath_checker.cpp +) + +target_compile_options(nexus_builtin_task_checkers PUBLIC INTERFACE cxx_std_17) + +target_link_libraries(nexus_builtin_task_checkers + PUBLIC + ${PROJECT_NAME} + nexus_common::nexus_common + nexus_endpoints::nexus_endpoints + pluginlib::pluginlib + rclcpp::rclcpp +) + +install( + TARGETS + nexus_builtin_task_checkers + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +pluginlib_export_plugin_description_file(nexus_capabilities src/task_checkers/plugins.xml) + ament_package() diff --git a/nexus_capabilities/include/nexus_capabilities/task_checker.hpp b/nexus_capabilities/include/nexus_capabilities/task_checker.hpp new file mode 100644 index 0000000..987436e --- /dev/null +++ b/nexus_capabilities/include/nexus_capabilities/task_checker.hpp @@ -0,0 +1,51 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef NEXUS_CAPABILITIES__TASK_CHECKER_HPP +#define NEXUS_CAPABILITIES__TASK_CHECKER_HPP + +#include "nexus_capabilities/task.hpp" + +#include "rclcpp/node_interfaces/node_interfaces.hpp" +#include "rclcpp/rclcpp.hpp" + +#include + +namespace nexus { +//============================================================================== +/** + * An implementation of this class can be used to verify a task can be performed. + */ +class TaskChecker +{ +public: + using NodeInterfaces = rclcpp::node_interfaces::NodeInterfaces; + + /// @brief Initialize the task checker. + /// @param interfaces Node interfaces that may be used for initialization. + virtual void initialize(NodeInterfaces interfaces) = 0; + + /// @brief Check if a task request is doable. + /// @param req The requested task. + virtual bool is_task_doable(const Task& task) = 0; + + virtual ~TaskChecker() = default; +}; + +} // namespace nexus + +#endif // NEXUS_CAPABILITIES__TASK_CHECKER_HPP diff --git a/nexus_capabilities/src/task_checkers/filepath_checker.cpp b/nexus_capabilities/src/task_checkers/filepath_checker.cpp new file mode 100644 index 0000000..2e22bb4 --- /dev/null +++ b/nexus_capabilities/src/task_checkers/filepath_checker.cpp @@ -0,0 +1,73 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include "nexus_capabilities/task_checker.hpp" + +#include "nexus_common/bt_store.hpp" + +#include +#include + +namespace nexus::task_checkers { + +/** + * A task a doable if a BT with the same name as the task type exists in the local filesystem. + */ +///============================================================================= +class FilepathChecker : public TaskChecker +{ +public: + /** + * @copydoc TaskChecker::initialize + */ + void initialize(NodeInterfaces interfaces) + { + auto param_interface = interfaces.get(); + // We assume that the parameter is already declared. + const std::string bt_path = param_interface->get_parameter("bt_path").get_value(); + this->_bt_store.register_from_path(bt_path); + } + + /** + * @copydoc TaskChecker::is_task_doable + */ + bool is_task_doable(const Task & task) + { + try + { + this->_bt_store.get_bt(task.type); + return true; + } + catch (const std::out_of_range&) + { + return false; + } + } + +private: + common::BTStore _bt_store; +}; + +} // namespace nexus::task_checkers + +///============================================================================= +#include + +PLUGINLIB_EXPORT_CLASS( + nexus::task_checkers::FilepathChecker, + nexus::TaskChecker +) \ No newline at end of file diff --git a/nexus_capabilities/src/task_checkers/plugins.xml b/nexus_capabilities/src/task_checkers/plugins.xml new file mode 100644 index 0000000..49ebf22 --- /dev/null +++ b/nexus_capabilities/src/task_checkers/plugins.xml @@ -0,0 +1,5 @@ + + + A task a doable if a BT with the same name as the task type exists in the local filesystem. + + diff --git a/nexus_integration_tests/launch/launch.py b/nexus_integration_tests/launch/launch.py index f3b11fe..ff4567b 100644 --- a/nexus_integration_tests/launch/launch.py +++ b/nexus_integration_tests/launch/launch.py @@ -123,6 +123,7 @@ def launch_setup(context, *args, **kwargs): FindPackageShare("nexus_integration_tests"), "/config/workcell_1_bts", ), + "task_checker_plugin": "nexus::task_checkers::FilepathChecker", "ros_domain_id": str(workcell_1_domain_id), "headless": headless, "use_zenoh_bridge": use_zenoh_bridge, @@ -165,6 +166,7 @@ def launch_setup(context, *args, **kwargs): FindPackageShare("nexus_integration_tests"), "/config/workcell_2_bts", ), + "task_checker_plugin": "nexus::task_checkers::FilepathChecker", "ros_domain_id": str(workcell_2_domain_id), "headless": headless, "use_zenoh_bridge": use_zenoh_bridge, diff --git a/nexus_integration_tests/launch/workcell.launch.py b/nexus_integration_tests/launch/workcell.launch.py index 3cc1b70..8498399 100644 --- a/nexus_integration_tests/launch/workcell.launch.py +++ b/nexus_integration_tests/launch/workcell.launch.py @@ -75,6 +75,7 @@ def launch_setup(context, *args, **kwargs): # Initialize launch configuration workcell_id = LaunchConfiguration("workcell_id") bt_path = LaunchConfiguration("bt_path") + task_checker_plugin = LaunchConfiguration("task_checker_plugin") ros_domain_id = LaunchConfiguration("ros_domain_id") headless = LaunchConfiguration("headless") controller_config_package = LaunchConfiguration("controller_config_package") @@ -150,6 +151,7 @@ def launch_setup(context, *args, **kwargs): ] ), Parameter("bt_path", bt_path), + Parameter("task_checker_plugin", task_checker_plugin), Parameter( "hardware_nodes", [ @@ -285,6 +287,10 @@ def generate_launch_description(): "bt_path", description="Path to load the BTs from", ), + DeclareLaunchArgument( + "task_checker_plugin", + description="Fully qualified name of the plugin to load to check if a task is doable.", + ), DeclareLaunchArgument( "ros_domain_id", default_value="0", diff --git a/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp b/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp index 63adf07..7ab1438 100644 --- a/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp +++ b/nexus_workcell_orchestrator/src/workcell_orchestrator.cpp @@ -63,7 +63,8 @@ static constexpr std::chrono::seconds REGISTER_TICK_RATE{1}; WorkcellOrchestrator::WorkcellOrchestrator(const rclcpp::NodeOptions& options) : rclcpp_lifecycle::LifecycleNode("workcell_orchestrator", options), - _capability_loader("nexus_capabilities", "nexus::Capability") + _capability_loader("nexus_capabilities", "nexus::Capability"), + _task_checker_loader("nexus_capabilities", "nexus::TaskChecker") { common::configure_logging(this); @@ -85,7 +86,6 @@ WorkcellOrchestrator::WorkcellOrchestrator(const rclcpp::NodeOptions& options) { throw std::runtime_error("param [bt_path] is required"); } - this->_bt_path = std::move(param); } { ParameterDescriptor desc; @@ -144,6 +144,19 @@ WorkcellOrchestrator::WorkcellOrchestrator(const rclcpp::NodeOptions& options) "Workcell can only handle generic tasks because it has no capabilities."); } + { + ParameterDescriptor desc; + desc.read_only = true; + desc.description = + "Fully qualified plugin name for the TaskChecker."; + auto param = this->declare_parameter( + "task_checker_plugin", "nexus::task_checkers::FilepathChecker", desc); + if (param.empty()) + { + throw std::runtime_error("param [task_checker_plugin] is required."); + } + } + this->_register_workcell_client = this->create_client( endpoints::RegisterWorkcellService::service_name()); @@ -243,6 +256,7 @@ auto WorkcellOrchestrator::on_cleanup( this->_queue_task_srv.reset(); this->_pause_srv.reset(); this->_signal_wc_srv.reset(); + this->_task_checker.reset(); this->_task_doable_srv.reset(); this->_cmd_server.reset(); this->_ctx_mgr.reset(); @@ -265,6 +279,25 @@ auto WorkcellOrchestrator::_configure( auto bt_path = this->get_parameter("bt_path").get_value(); this->_bt_store.register_from_path(bt_path); + try + { + const std::string plugin_name = this->get_parameter("task_checker_plugin").get_value(); + RCLCPP_INFO( + this->get_logger(), + "Loading TaskChecker plugin [ %s ].", + plugin_name.c_str() + ); + _task_checker = _task_checker_loader.createSharedInstance(plugin_name); + _task_checker->initialize(*this); + } + catch(pluginlib::PluginlibException& ex) + { + RCLCPP_ERROR( + this->get_logger(), + "Failed to load TaskChecker plugin. Error: %s\n", ex.what()); + return CallbackReturn::FAILURE; + } + // create workcell request action server this->_cmd_server = rclcpp_action::create_server( @@ -347,7 +380,7 @@ auto WorkcellOrchestrator::_configure( try { ctx->task = this->_task_parser.parse_task(goal->task); - if (!this->_can_perform_task( + if (!this->_task_checker->is_task_doable( ctx->task)) { auto result = std::make_shared(); @@ -997,7 +1030,7 @@ void WorkcellOrchestrator::_handle_task_doable( try { auto task = this->_task_parser.parse_task(req->task); - resp->success = this->_can_perform_task(task); + resp->success = this->_task_checker->is_task_doable(task); if (resp->success) { RCLCPP_DEBUG(this->get_logger(), "Workcell can perform task"); @@ -1016,19 +1049,6 @@ void WorkcellOrchestrator::_handle_task_doable( } } -bool WorkcellOrchestrator::_can_perform_task(const Task& task) -{ - try - { - this->_bt_store.get_bt(task.type); - return true; - } - catch (const std::out_of_range&) - { - return false; - } -} - } #include diff --git a/nexus_workcell_orchestrator/src/workcell_orchestrator.hpp b/nexus_workcell_orchestrator/src/workcell_orchestrator.hpp index 8e2b539..30813bc 100644 --- a/nexus_workcell_orchestrator/src/workcell_orchestrator.hpp +++ b/nexus_workcell_orchestrator/src/workcell_orchestrator.hpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -123,11 +124,11 @@ private: std::list> _ctxs; private: std::shared_ptr _ctx_mgr; private: std::unique_ptr> _lifecycle_mgr{ nullptr}; -private: std::filesystem::path _bt_path; - // mapping of mapped task type and the original +// mapping of mapped task type and the original private: std::unordered_map _task_remaps; private: TaskParser _task_parser; - +private: pluginlib::ClassLoader _task_checker_loader; +private: std::shared_ptr _task_checker; private: CallbackReturn _configure( const rclcpp_lifecycle::State& previous_state); @@ -165,8 +166,6 @@ private: void _handle_command_failed(const std::shared_ptr& ctx); private: void _handle_task_doable( endpoints::IsTaskDoableService::ServiceType::Request::ConstSharedPtr req, endpoints::IsTaskDoableService::ServiceType::Response::SharedPtr resp); - -private: bool _can_perform_task(const Task& task); }; } From 7d77a81ae106db97d5bc79d51221afa092e91597 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 6 Jan 2025 14:04:54 +0800 Subject: [PATCH 3/3] Make main behavior tree configurable (#47) * Remove main behavior tree Signed-off-by: Luca Della Vedova * Remove task remaps from system orchestrator Signed-off-by: Luca Della Vedova * Add parameter for bt filename and parameter callback Signed-off-by: Luca Della Vedova * Update integration test launch Signed-off-by: Luca Della Vedova * Remove unused variable Signed-off-by: Luca Della Vedova * Restore main.xml Signed-off-by: Luca Della Vedova * bt_filename -> main_bt_filename Signed-off-by: Luca Della Vedova * Make main_bt_filename a launch parameter Signed-off-by: Luca Della Vedova --------- Signed-off-by: Luca Della Vedova --- .../launch/control_center.launch.py | 7 +++ .../src/system_orchestrator.cpp | 54 ++++++++++++++++--- .../src/system_orchestrator.hpp | 7 +++ 3 files changed, 61 insertions(+), 7 deletions(-) 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.