From 0ca90201bb865411656956fa9c65476ed83e79c4 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 6 Jan 2025 11:56:50 +0800 Subject: [PATCH] Restore main.xml Signed-off-by: Luca Della Vedova --- .../config/system_bts/main.xml | 21 +++++ .../config/system_bts/pick_and_place.xml | 32 ++------ .../launch/control_center.launch.py | 4 + nexus_system_orchestrator/CMakeLists.txt | 1 + nexus_system_orchestrator/src/context.hpp | 2 + .../src/execute_task.cpp | 82 +++++++++++++++++++ .../src/execute_task.hpp | 75 +++++++++++++++++ .../src/system_orchestrator.cpp | 38 +++++++-- .../src/system_orchestrator.hpp | 2 + 9 files changed, 227 insertions(+), 30 deletions(-) create mode 100644 nexus_integration_tests/config/system_bts/main.xml create mode 100644 nexus_system_orchestrator/src/execute_task.cpp create mode 100644 nexus_system_orchestrator/src/execute_task.hpp diff --git a/nexus_integration_tests/config/system_bts/main.xml b/nexus_integration_tests/config/system_bts/main.xml new file mode 100644 index 0000000..3f96eaf --- /dev/null +++ b/nexus_integration_tests/config/system_bts/main.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/nexus_integration_tests/config/system_bts/pick_and_place.xml b/nexus_integration_tests/config/system_bts/pick_and_place.xml index 8e69348..07a7e78 100644 --- a/nexus_integration_tests/config/system_bts/pick_and_place.xml +++ b/nexus_integration_tests/config/system_bts/pick_and_place.xml @@ -2,30 +2,14 @@ - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + diff --git a/nexus_integration_tests/launch/control_center.launch.py b/nexus_integration_tests/launch/control_center.launch.py index ff53034..cd69082 100644 --- a/nexus_integration_tests/launch/control_center.launch.py +++ b/nexus_integration_tests/launch/control_center.launch.py @@ -128,6 +128,10 @@ def launch_setup(context, *args, **kwargs): FindPackageShare("nexus_integration_tests"), "/config/system_bts", ), + "remap_task_types": + """{ + pick_and_place: [place_on_conveyor, pick_from_conveyor], + }""", "bt_filename": "pick_and_place.xml", "max_jobs": 2, } diff --git a/nexus_system_orchestrator/CMakeLists.txt b/nexus_system_orchestrator/CMakeLists.txt index e474698..d3459bb 100644 --- a/nexus_system_orchestrator/CMakeLists.txt +++ b/nexus_system_orchestrator/CMakeLists.txt @@ -19,6 +19,7 @@ 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/context.hpp b/nexus_system_orchestrator/src/context.hpp index 412b4ae..d50470e 100644 --- a/nexus_system_orchestrator/src/context.hpp +++ b/nexus_system_orchestrator/src/context.hpp @@ -43,6 +43,8 @@ public: rclcpp_lifecycle::LifecycleNode& node; public: std::string job_id; public: WorkOrder wo; public: std::vector tasks; +public: std::shared_ptr> task_remaps; /** * Map of task ids and their assigned workcell ids. */ diff --git a/nexus_system_orchestrator/src/execute_task.cpp b/nexus_system_orchestrator/src/execute_task.cpp new file mode 100644 index 0000000..fbf626a --- /dev/null +++ b/nexus_system_orchestrator/src/execute_task.cpp @@ -0,0 +1,82 @@ +/* + * Copyright (C) 2023 Johnson & Johnson + * + * 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 "execute_task.hpp" + +namespace nexus::system_orchestrator { + +BT::NodeStatus ExecuteTask::onStart() +{ + auto task = this->getInput("task"); + if (!task) + { + RCLCPP_ERROR( + this->_ctx->node.get_logger(), "%s: [task] port is required", + this->name().c_str()); + return BT::NodeStatus::FAILURE; + } + + auto workcell = this->getInput("workcell"); + if (!workcell) + { + RCLCPP_ERROR( + this->_ctx->node.get_logger(), "%s: [workcell] port is required", + this->name().c_str()); + return BT::NodeStatus::FAILURE; + } + + // Remap the BT filename to load if one is provided. + std::string bt_name = task->type; + auto it = _ctx->task_remaps->find(task->type); + if (it != _ctx->task_remaps->end()) + { + RCLCPP_DEBUG( + _ctx->node.get_logger(), + "[ExecuteTask] Loading remapped BT [%s] for original task type [%s]", + it->second.c_str(), + task->type.c_str() + ); + bt_name = it->second; + } + std::filesystem::path task_bt_path(this->_bt_path / (bt_name + ".xml")); + if (!std::filesystem::is_regular_file(task_bt_path)) + { + RCLCPP_ERROR( + this->_ctx->node.get_logger(), "%s: no behavior tree to execute task type [%s]", + this->name().c_str(), task->type.c_str()); + return BT::NodeStatus::FAILURE; + } + + this->_bt = std::make_unique(this->_bt_factory->createTreeFromFile( + task_bt_path)); + this->_bt->rootBlackboard()->set("task", *task); + this->_bt->rootBlackboard()->set("workcell", *workcell); + + return BT::NodeStatus::RUNNING; +} + +BT::NodeStatus ExecuteTask::onRunning() +{ + return this->_bt->tickRoot(); +} + +void ExecuteTask::onHalted() +{ + this->_bt->haltTree(); +} + +} diff --git a/nexus_system_orchestrator/src/execute_task.hpp b/nexus_system_orchestrator/src/execute_task.hpp new file mode 100644 index 0000000..c3ce603 --- /dev/null +++ b/nexus_system_orchestrator/src/execute_task.hpp @@ -0,0 +1,75 @@ +/* + * Copyright (C) 2023 Johnson & Johnson + * + * 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_SYSTEM_ORCHESTRATOR__EXECUTE_TASK_HPP +#define NEXUS_SYSTEM_ORCHESTRATOR__EXECUTE_TASK_HPP + +#include "context.hpp" + +#include + +#include +#include + +#include + +namespace nexus::system_orchestrator { + +/** + * Searches for a behavior tree to execute a task and run it as a sub tree. + * + * The behavior tree used is based on the task type after remapping and the + * filename of the behavior tree without extension. + * + * Input Ports: + * task |nexus_orchestrator_msgs::msg::WorkcellTask| The task to execute. + * workcell |std::string| Workcell to execute on. + */ +class ExecuteTask : public BT::StatefulActionNode +{ +public: using Task = nexus_orchestrator_msgs::msg::WorkcellTask; + +public: static BT::PortsList providedPorts() + { + return { BT::InputPort("task"), + BT::InputPort("workcell") }; + } + +public: ExecuteTask(const std::string& name, + const BT::NodeConfiguration& config, + std::shared_ptr ctx, + std::filesystem::path bt_path, + std::shared_ptr bt_factory) + : BT::StatefulActionNode(name, config), _ctx(std::move(ctx)), _bt_path(std::move( + bt_path)), + _bt_factory(std::move(bt_factory)) {} + +protected: BT::NodeStatus onStart() override; + +protected: BT::NodeStatus onRunning() override; + +protected: void onHalted() override; + +private: std::shared_ptr _ctx; +private: std::filesystem::path _bt_path; +private: std::shared_ptr _bt_factory; +private: std::unique_ptr _bt; +}; + +} + +#endif diff --git a/nexus_system_orchestrator/src/system_orchestrator.cpp b/nexus_system_orchestrator/src/system_orchestrator.cpp index 8a7bdff..55d145c 100644 --- a/nexus_system_orchestrator/src/system_orchestrator.cpp +++ b/nexus_system_orchestrator/src/system_orchestrator.cpp @@ -20,6 +20,7 @@ #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" @@ -70,7 +71,7 @@ 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."; + "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()) { @@ -90,10 +91,6 @@ SystemOrchestrator::SystemOrchestrator(const rclcpp::NodeOptions& options) 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"); - } if (!this->_bt_filename_valid(this->_bt_filename)) { @@ -102,6 +99,26 @@ SystemOrchestrator::SystemOrchestrator(const rclcpp::NodeOptions& options) } } + { + _task_remaps = + std::make_shared>(); + ParameterDescriptor desc; + desc.read_only = true; + desc.description = + "A yaml containing a dictionary of task types and an array of remaps."; + const auto yaml = this->declare_parameter("remap_task_types", "", desc); + const auto remaps = YAML::Load(yaml); + for (const auto& n : remaps) + { + const auto task_type = n.first.as(); + const auto& mappings = n.second; + for (const auto& m : mappings) + { + this->_task_remaps->emplace(m.as(), task_type); + } + } + } + { ParameterDescriptor desc; desc.read_only = true; @@ -150,6 +167,7 @@ SystemOrchestrator::SystemOrchestrator(const rclcpp::NodeOptions& options) } return result; }); + } auto SystemOrchestrator::on_configure(const rclcpp_lifecycle::State& previous) @@ -502,6 +520,14 @@ BT::Tree SystemOrchestrator::_create_bt(const WorkOrderActionType::Goal& wo, this->get_logger(), ctx); }); + bt_factory->registerBuilder("ExecuteTask", + [this, ctx, bt_factory](const std::string& name, + const BT::NodeConfiguration& config) + { + return std::make_unique(name, config, ctx, this->_bt_path, + bt_factory); + }); + bt_factory->registerBuilder("SendSignal", [ctx](const std::string& name, const BT::NodeConfiguration& config) { @@ -519,7 +545,7 @@ void SystemOrchestrator::_create_job(const WorkOrderActionType::Goal& goal) // using `new` because make_shared does not work with aggregate initializer std::shared_ptr ctx{new Context{*this, - goal.order.id, wo, tasks, + goal.order.id, wo, tasks, this->_task_remaps, std::unordered_map{}, this->_workcell_sessions, this->_transporter_sessions, {}, nullptr, diff --git a/nexus_system_orchestrator/src/system_orchestrator.hpp b/nexus_system_orchestrator/src/system_orchestrator.hpp index c11eaf6..b2e4789 100644 --- a/nexus_system_orchestrator/src/system_orchestrator.hpp +++ b/nexus_system_orchestrator/src/system_orchestrator.hpp @@ -96,6 +96,8 @@ class SystemOrchestrator : public std::unique_ptr> _lifecycle_mgr{nullptr}; rclcpp::TimerBase::SharedPtr _pre_configure_timer; rclcpp::SubscriptionBase::SharedPtr _estop_sub; + // mapping of mapped task type and the original + std::shared_ptr> _task_remaps; std::shared_ptr _param_cb_handle; /**