From 580244121d777f31cf7161f8ad284c756da5ae4d Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Thu, 12 Dec 2024 12:07:45 +0800 Subject: [PATCH] Go back to task signaling Signed-off-by: Luca Della Vedova --- .../config/system_bts/main.xml | 12 +-- .../config/system_bts/pick_and_place.xml | 2 +- .../src/dispatch_transporter.cpp | 10 +++ .../src/execute_task.cpp | 5 ++ .../src/execute_task.hpp | 3 +- nexus_system_orchestrator/src/signals.cpp | 84 +++++++++---------- nexus_system_orchestrator/src/signals.hpp | 3 - .../src/workcell_request.cpp | 4 +- 8 files changed, 67 insertions(+), 56 deletions(-) diff --git a/nexus_integration_tests/config/system_bts/main.xml b/nexus_integration_tests/config/system_bts/main.xml index e7311cb..3588eef 100644 --- a/nexus_integration_tests/config/system_bts/main.xml +++ b/nexus_integration_tests/config/system_bts/main.xml @@ -3,16 +3,16 @@ - - + + + + + + - - - - 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 b36f4a2..c413801 100644 --- a/nexus_integration_tests/config/system_bts/pick_and_place.xml +++ b/nexus_integration_tests/config/system_bts/pick_and_place.xml @@ -6,7 +6,7 @@ - + diff --git a/nexus_system_orchestrator/src/dispatch_transporter.cpp b/nexus_system_orchestrator/src/dispatch_transporter.cpp index 7234cfc..f320670 100644 --- a/nexus_system_orchestrator/src/dispatch_transporter.cpp +++ b/nexus_system_orchestrator/src/dispatch_transporter.cpp @@ -15,6 +15,8 @@ * */ +#include + #include #include "dispatch_transporter.hpp" @@ -54,6 +56,7 @@ BT::NodeStatus DispatchTransporter::onStart() } // Multipickup task // TODO(luca) Consider encoding where is a pickup and where a dropoff + // TODO(luca) remove consecutive duplicates (multiple tasks to same workcell that don't need transportation) YAML::Node order; order["type"] = "pickup"; order["destination"] = assignment_it->second; @@ -151,6 +154,13 @@ BT::NodeStatus DispatchTransporter::_update_ongoing_requests() workcell_id.c_str()); this->setOutput("result", workcell_id); this->setOutput("transport_task", this->_transport_task); + // Update the context + const auto& task_id = this->_transport_task.id; + this->_ctx->workcell_task_assignments.emplace(task_id, workcell_id); + auto p = this->_ctx->task_states.emplace(task_id, nexus_orchestrator_msgs::msg::TaskState()); + auto& task_state = p.first->second; + task_state.workcell_id = workcell_id; + task_state.task_id = task_id; return BT::NodeStatus::SUCCESS; } else diff --git a/nexus_system_orchestrator/src/execute_task.cpp b/nexus_system_orchestrator/src/execute_task.cpp index fbf626a..f34c33d 100644 --- a/nexus_system_orchestrator/src/execute_task.cpp +++ b/nexus_system_orchestrator/src/execute_task.cpp @@ -65,6 +65,11 @@ BT::NodeStatus ExecuteTask::onStart() task_bt_path)); this->_bt->rootBlackboard()->set("task", *task); this->_bt->rootBlackboard()->set("workcell", *workcell); + auto transport_task = this->getInput("transport_task"); + if (transport_task) + { + this->_bt->rootBlackboard()->set("transport_task", *transport_task); + } return BT::NodeStatus::RUNNING; } diff --git a/nexus_system_orchestrator/src/execute_task.hpp b/nexus_system_orchestrator/src/execute_task.hpp index c3ce603..63cfc94 100644 --- a/nexus_system_orchestrator/src/execute_task.hpp +++ b/nexus_system_orchestrator/src/execute_task.hpp @@ -46,7 +46,8 @@ public: using Task = nexus_orchestrator_msgs::msg::WorkcellTask; public: static BT::PortsList providedPorts() { return { BT::InputPort("task"), - BT::InputPort("workcell") }; + BT::InputPort("workcell"), + BT::InputPort("transport_task") }; } public: ExecuteTask(const std::string& name, diff --git a/nexus_system_orchestrator/src/signals.cpp b/nexus_system_orchestrator/src/signals.cpp index c00f3d9..5379d2f 100644 --- a/nexus_system_orchestrator/src/signals.cpp +++ b/nexus_system_orchestrator/src/signals.cpp @@ -23,11 +23,12 @@ namespace nexus::system_orchestrator { BT::NodeStatus SendSignal::tick() { - auto workcell = this->getInput("workcell"); - if (!workcell) + auto task = + this->getInput("task"); + if (!task) { RCLCPP_ERROR( - this->_ctx->node.get_logger(), "%s [workcell] is required", + this->_ctx->node.get_logger(), "%s: port [task] is required", this->name().c_str()); return BT::NodeStatus::FAILURE; } @@ -41,48 +42,47 @@ BT::NodeStatus SendSignal::tick() return BT::NodeStatus::FAILURE; } - return signal_workcell(*workcell, *signal); -} - -BT::NodeStatus SendSignal::signal_workcell(const std::string& workcell, const std::string& signal) -{ - auto it = this->_ctx->workcell_sessions.find(workcell); - if (it == this->_ctx->workcell_sessions.cend()) + try { - RCLCPP_ERROR( - this->_ctx->node.get_logger(), - "%s: Unable to find workcell [%s]", - this->name().c_str(), workcell.c_str()); - return BT::NodeStatus::FAILURE; - } - const auto& session = it->second; - if (!session) - { - // TODO(luca) implement a queueing mechanism if the workcell is not available? - RCLCPP_WARN( - this->_ctx->node.get_logger(), - "%s: No session found for workcell [%s], skipping signaling", - this->name().c_str(), workcell.c_str()); - return BT::NodeStatus::SUCCESS; - } + auto it = std::find_if( + this->_ctx->workcell_task_assignments.cbegin(), + this->_ctx->workcell_task_assignments.cend(), + [&task](const std::pair& p) + { + return p.first == task->id; + }); + if (it == this->_ctx->workcell_task_assignments.cend()) + { + RCLCPP_ERROR( + this->_ctx->node.get_logger(), "%s: Unable to find workcell assigned to task [%s]", + this->name().c_str(), task->id.c_str()); + return BT::NodeStatus::FAILURE; + } + const auto& workcell = it->second; - auto req = - std::make_shared(); - // TODO(luca) Using job id for task id is not scalable, generate uuids? - req->task_id = this->_ctx->job_id; - req->signal = signal; - auto resp = session->signal_wc_client->send_request(req); - RCLCPP_INFO( - this->_ctx->node.get_logger(), "%s: Sent signal [%s] to workcell [%s]", - this->name().c_str(), signal.c_str(), workcell.c_str()); - if (!resp->success) + const auto& session = this->_ctx->workcell_sessions.at(workcell); + auto req = + std::make_shared(); + req->task_id = task->id; + req->signal = *signal; + auto resp = session->signal_wc_client->send_request(req); + RCLCPP_INFO( + this->_ctx->node.get_logger(), "%s: Sent signal [%s] to workcell [%s]", + this->name().c_str(), signal->c_str(), workcell.c_str()); + if (!resp->success) + { + RCLCPP_WARN( + this->_ctx->node.get_logger(), + "%s: Workcell is not able to accept [%s] signal now. Queuing the signal to be sent on the next task request.", + this->name().c_str(), signal->c_str()); + this->_ctx->queued_signals[task->id].emplace_back(*signal); + } + } + catch (const std::out_of_range& e) { - // TODO(luca) implement a queueing mechanism if the request fails? - RCLCPP_WARN( - this->_ctx->node.get_logger(), - "%s: Workcell [%s] is not able to accept [%s] signal now. Skipping signaling, error: [%s]", - this->name().c_str(), workcell.c_str(), signal.c_str(), resp->message.c_str()); - return BT::NodeStatus::SUCCESS; + RCLCPP_ERROR(this->_ctx->node.get_logger(), "%s: %s", + this->name().c_str(), e.what()); + return BT::NodeStatus::FAILURE; } return BT::NodeStatus::SUCCESS; diff --git a/nexus_system_orchestrator/src/signals.hpp b/nexus_system_orchestrator/src/signals.hpp index 89dc40b..01c31f3 100644 --- a/nexus_system_orchestrator/src/signals.hpp +++ b/nexus_system_orchestrator/src/signals.hpp @@ -40,7 +40,6 @@ public: static BT::PortsList providedPorts() { return { BT::InputPort("task", "The task the signal is tied to."), - BT::InputPort("workcell", "The workcell to signal"), BT::InputPort( "signal", "Signal to send.") }; } @@ -51,8 +50,6 @@ public: SendSignal(const std::string& name, const BT::NodeConfiguration& config, public: BT::NodeStatus tick() override; -private: BT::NodeStatus signal_workcell(const std::string& workcell, const std::string& signal); - private: std::shared_ptr _ctx; }; diff --git a/nexus_system_orchestrator/src/workcell_request.cpp b/nexus_system_orchestrator/src/workcell_request.cpp index bc41d7a..e53bec4 100644 --- a/nexus_system_orchestrator/src/workcell_request.cpp +++ b/nexus_system_orchestrator/src/workcell_request.cpp @@ -102,9 +102,7 @@ WorkcellRequest::make_goal() void WorkcellRequest::on_feedback( endpoints::WorkcellRequestAction::ActionType::Feedback::ConstSharedPtr msg) { - // Will insert a new state if one was not created at work order creation time - // i.e. for transportation requests that are done after task assignments - this->_ctx->task_states[this->_task.id] = msg->state; + this->_ctx->task_states.at(this->_task.id) = msg->state; this->_on_task_progress(this->_ctx->task_states); }