#63 part 1

Signed-off-by: Aaron Chong <[email protected]>

nexus_capabilities/include/nexus_capabilities/task.hpp

@@ -30,7 +30,8 @@ using TaskData = YAML::Node;
 
 struct Task
 {
-public: std::string id;
+public: std::string work_order_id;
+public: std::string task_id;
 public: std::string type;
 public: TaskData data;
 public: YAML::Node previous_results;

nexus_capabilities/src/capabilities/dispense_item_task_data.hpp

@@ -18,8 +18,6 @@
 #ifndef NEXUS_CAPABILITIES__CAPABILITIES__DISPENSE_ITEM_TASK_DATA_HPP
 #define NEXUS_CAPABILITIES__CAPABILITIES__DISPENSE_ITEM_TASK_DATA_HPP
 
-#include <nexus_capabilities/task.hpp>
-
 #include <yaml-cpp/yaml.h>
 
 namespace nexus::capabilities {

nexus_msgs/nexus_orchestrator_msgs/msg/WorkcellState.msg

@@ -1,10 +1,13 @@
 # Unique identifier for workcell
 string workcell_id
 
+# Current work order being fulfilled
+string work_order_id
+
 # Current task being performed
 string task_id
 
-# [OPTIONAL] message for debugging 
+# [OPTIONAL] message for debugging
 string message
 
 uint8 status

nexus_msgs/nexus_orchestrator_msgs/msg/WorkcellTask.msg

@@ -1,4 +1,8 @@
-string id
+# Work order of the task
+string work_order_id
+
+# Unique ID of the task
+string task_id
 
 # Type of the task
 string type

nexus_system_orchestrator/src/assign_transporter_workcell.cpp

@@ -49,12 +49,12 @@ BT::NodeStatus AssignTransporterWorkcell::onStart()
 
   for (const auto& task : this->_ctx->tasks)
   {
-    auto assignment_it = task_assignments.find(task.id);
+    auto assignment_it = task_assignments.find(task.task_id);
     if (assignment_it == task_assignments.end())
     {
       RCLCPP_ERROR(
         node->get_logger(), "%s: Unable to transport, task [%s] was not assigned to a workcell",
-        this->name().c_str(), task.id.c_str());
+        this->name().c_str(), task.task_id.c_str());
       return BT::NodeStatus::FAILURE;
     }
     // Multipickup task
@@ -63,10 +63,11 @@ BT::NodeStatus AssignTransporterWorkcell::onStart()
     YAML::Node order;
     order["type"] = "pickup";
     order["destination"] = assignment_it->second;
-    order["workcell_task_id"] = task.id;
+    order["workcell_task_id"] = task.task_id;
     orders.push_back(order);
   }
-  this->_transport_task.id = this->_ctx->job_id;
+  this->_transport_task.work_order_id = this->_ctx->job_id;
+  this->_transport_task.task_id = this->_ctx->job_id;
   this->_transport_task.type = "transportation";
   YAML::Emitter out;
   out << orders;
@@ -159,7 +160,7 @@ BT::NodeStatus AssignTransporterWorkcell::_update_ongoing_requests()
         this->setOutput("transporter_id", workcell_id);
         this->setOutput("transport_task", this->_transport_task);
         // Update the context
-        const auto& task_id = this->_transport_task.id;
+        const auto& task_id = this->_transport_task.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;

nexus_system_orchestrator/src/for_each_task.cpp

@@ -51,12 +51,12 @@ BT::NodeStatus ForEachTask::tick()
     try
     {
       this->setOutput("workcell",
-        this->_ctx->workcell_task_assignments.at(current_task.id));
+        this->_ctx->workcell_task_assignments.at(current_task.task_id));
     }
     catch (const std::out_of_range&)
     {
       RCLCPP_ERROR(this->_logger, "task [%s] not assigned to any workcell",
-        current_task.id.c_str());
+        current_task.task_id.c_str());
       return BT::NodeStatus::FAILURE;
     }
 

nexus_system_orchestrator/src/send_signal.cpp

@@ -49,21 +49,21 @@ BT::NodeStatus SendSignal::tick()
       this->_ctx->workcell_task_assignments.cend(),
       [&task](const std::pair<std::string, std::string>& p)
       {
-        return p.first == task->id;
+        return p.first == task->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());
+        this->name().c_str(), task->task_id.c_str());
       return BT::NodeStatus::FAILURE;
     }
     const auto& workcell = it->second;
 
     const auto& session = this->_ctx->workcell_sessions.at(workcell);
     auto req =
       std::make_shared<endpoints::SignalWorkcellService::ServiceType::Request>();
-    req->task_id = task->id;
+    req->task_id = task->task_id;
     req->signal = *signal;
     auto resp = session->signal_wc_client->send_request(req);
     RCLCPP_INFO(
@@ -75,7 +75,7 @@ BT::NodeStatus SendSignal::tick()
         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);
+      this->_ctx->queued_signals[task->task_id].emplace_back(*signal);
     }
   }
   catch (const std::out_of_range& e)

nexus_system_orchestrator/src/system_orchestrator.cpp

@@ -552,7 +552,7 @@ void SystemOrchestrator::_create_job(const WorkOrderActionType::Goal& goal)
 {
   auto wo =
     YAML::Load(goal.order.work_order).as<common::WorkOrder>();
-  auto tasks = this->_parse_wo(wo);
+  auto tasks = this->_parse_wo(goal.order.id, wo);
 
   // using `new` because make_shared does not work with aggregate initializer
   std::shared_ptr<Context> ctx{new Context{*this,
@@ -684,15 +684,16 @@ void SystemOrchestrator::_init_job(
 }
 
 std::vector<nexus_orchestrator_msgs::msg::WorkcellTask> SystemOrchestrator::
-_parse_wo(const common::WorkOrder& work_order)
+_parse_wo(const std::string& work_order_id, const common::WorkOrder& work_order)
 {
   std::vector<nexus_orchestrator_msgs::msg::WorkcellTask> tasks;
   const auto steps = work_order.steps();
   tasks.reserve(steps.size());
   for (const auto& step : steps)
   {
     nexus_orchestrator_msgs::msg::WorkcellTask task;
-    task.id = std::to_string(step.id());
+    task.work_order_id = work_order_id;
+    task.task_id = std::to_string(step.id());
     task.type = step.process_id();
 
     // FIXME(koonpeng): data from arcstone is missing the work order item,
@@ -764,6 +765,7 @@ void SystemOrchestrator::_handle_register_workcell(
   const auto& workcell_id = req->description.workcell_id;
   WorkcellState state;
   state.workcell_id = workcell_id;
+  state.work_order_id = "";
   state.status = WorkcellState::STATUS_IDLE;
   this->_workcell_sessions.emplace(workcell_id,
     std::make_shared<WorkcellSession>(WorkcellSession{
@@ -1049,14 +1051,14 @@ void SystemOrchestrator::_assign_workcell_task(const WorkcellTask& task,
       if (assigned.empty())
       {
         RCLCPP_ERROR(this->get_logger(),
-        "No workcell is able perform task [%s]", task.id.c_str());
+        "No workcell is able perform task [%s]", task.task_id.c_str());
         on_done(std::nullopt);
       }
       else
       {
         RCLCPP_INFO(
           this->get_logger(), "Task [%s] assigned to workcell [%s]",
-          task.id.c_str(), assigned.c_str());
+          task.task_id.c_str(), assigned.c_str());
         on_done(assigned);
       }
     });
@@ -1076,7 +1078,7 @@ void SystemOrchestrator::_assign_all_tasks(
       [on_done, num_tasks, task_assignments, task](
         const std::optional<std::string>& assigned)
       {
-        task_assignments->emplace(task.id, assigned);
+        task_assignments->emplace(task.task_id, assigned);
         if (task_assignments->size() == num_tasks)
         {
           on_done(*task_assignments);

nexus_system_orchestrator/src/system_orchestrator.hpp

@@ -123,6 +123,7 @@ class SystemOrchestrator : public
   void _init_job(const std::shared_ptr<WorkOrderGoalHandle> goal_handle);
 
   std::vector<nexus_orchestrator_msgs::msg::WorkcellTask> _parse_wo(
+    const std::string& work_order_id,
     const common::WorkOrder& work_order);
 
   void _handle_wo_cancel(

nexus_system_orchestrator/src/workcell_request.cpp

@@ -81,15 +81,15 @@ WorkcellRequest::make_goal()
   goal.task = this->_task;
   try
   {
-    auto& signals = this->_ctx->queued_signals.at(this->_task.id);
+    auto& signals = this->_ctx->queued_signals.at(this->_task.task_id);
     goal.start_signals = signals;
     signals.clear();
   }
   catch (const std::out_of_range&)
   {
     RCLCPP_DEBUG(
       this->_ctx->node.get_logger(), "%s: No queued signals for task [%s]",
-      this->name().c_str(), this->_task.id.c_str());
+      this->name().c_str(), this->_task.task_id.c_str());
     // ignore
   }
   goal.task.previous_results = this->_ctx->task_results;
@@ -102,7 +102,7 @@ WorkcellRequest::make_goal()
 void WorkcellRequest::on_feedback(
   endpoints::WorkcellRequestAction::ActionType::Feedback::ConstSharedPtr msg)
 {
-  this->_ctx->task_states.at(this->_task.id) = msg->state;
+  this->_ctx->task_states.at(this->_task.task_id) = msg->state;
   this->_on_task_progress(this->_ctx->task_states);
 }
 
@@ -125,7 +125,7 @@ bool WorkcellRequest::on_result(
   }
 
   this->_ctx->task_results = result.result->result; // -.-
-  this->_ctx->task_states.at(this->_task.id).status =
+  this->_ctx->task_states.at(this->_task.task_id).status =
     TaskState::STATUS_FINISHED;
   this->_on_task_progress(this->_ctx->task_states);
   return true;

nexus_workcell_orchestrator/src/task_parser.cpp

@@ -28,7 +28,8 @@ Task TaskParser::parse_task(
   const nexus_orchestrator_msgs::msg::WorkcellTask& workcell_task)
 {
   return Task{
-    workcell_task.id,
+    workcell_task.work_order_id,
+    workcell_task.task_id,
     workcell_task.type,
     YAML::Load(workcell_task.payload),
     YAML::Load(workcell_task.previous_results),