Work order ID and task ID (#71)

* #63 part 1 Signed-off-by: Aaron Chong <[email protected]> * Work order ID published in state Signed-off-by: Aaron Chong <[email protected]> * Remove id from steps, use {work_order_id}/{process_id} for task id Signed-off-by: Aaron Chong <[email protected]> * Removed more unused step ID, set work order ID in tests and fix tests Signed-off-by: Aaron Chong <[email protected]> * Remove id in WorkOrder msg, changed number to id in WorkOrder model, use that for work_order_id Signed-off-by: Aaron Chong <[email protected]> * Correct tests Signed-off-by: Aaron Chong <[email protected]> * Use action goal IDs, and generate step indexes, update tests, add new parallel duplicate wo test Signed-off-by: Aaron Chong <[email protected]> * Fix uuid parsing, use released zenoh bridge to speed up tests, test not killing zenoh Signed-off-by: Aaron Chong <[email protected]> * Revert Signed-off-by: Aaron Chong <[email protected]> * Reinstate id in WorkOrder msg, remove use of goal uuid Signed-off-by: Aaron Chong <[email protected]> * Address feedback Signed-off-by: Aaron Chong <[email protected]> * Fix tests Signed-off-by: Aaron Chong <[email protected]> --------- Signed-off-by: Aaron Chong <[email protected]>
osrf · Feb 21, 2025 · c4f8421 · c4f8421
1 parent 59a59e0
commit c4f8421
Show file tree

Hide file tree

Showing 25 changed files with 191 additions and 94 deletions.
diff --git a/nexus_capabilities/include/nexus_capabilities/task.hpp b/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;

diff --git a/nexus_capabilities/src/capabilities/dispense_item_task_data.hpp b/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 {

diff --git a/nexus_common/include/nexus_common/models/work_order.hpp b/nexus_common/include/nexus_common/models/work_order.hpp
@@ -68,11 +68,6 @@ public: struct Step
 
     Step() {}
 
-    int64_t id() const
-    {
-      return this->yaml["id"].as<double>();
-    }
-
     std::string process_id() const
     {
       return this->yaml["processId"].as<std::string>();
@@ -91,11 +86,6 @@ public: WorkOrder(YAML::Node yaml)
 
 public: WorkOrder() {}
 
-public: std::string number() const
-  {
-    return this->yaml["number"].as<std::string>();
-  }
-
 public: std::string work_instruction_name() const
   {
     return this->yaml["workInstructionName"].as<std::string>();

diff --git a/nexus_common/src/models/work_order_test.cpp b/nexus_common/src/models/work_order_test.cpp
@@ -30,7 +30,6 @@ TEST_CASE("WorkOrder serialization", "[Model][Serialization]")
   std::string raw{
     R"RAW(
       {
-        "number": "SO/2022/20/1-1",
         "workInstructionName": "CV-299 (Rev 4)",
         "item": {
           "SkuId": "1001",
@@ -41,17 +40,14 @@ TEST_CASE("WorkOrder serialization", "[Model][Serialization]")
         },
         "steps": [
           {
-            "id": 31.0,
             "processId": "pickup",
             "name": "pickup item"
           },
           {
-            "id": 32.0,
             "processId": "place",
             "name": "place item"
           },
           {
-            "id": 33.0,
             "processId": "inspect",
             "name": "inspect item"
           }
@@ -61,7 +57,6 @@ TEST_CASE("WorkOrder serialization", "[Model][Serialization]")
 
   auto check_data = [](const WorkOrder& work_order)
     {
-      CHECK(work_order.number() == "SO/2022/20/1-1");
       CHECK(work_order.work_instruction_name() == "CV-299 (Rev 4)");
       const auto item = work_order.item();
       CHECK(item.sku_id() == "1001");
@@ -74,18 +69,15 @@ TEST_CASE("WorkOrder serialization", "[Model][Serialization]")
       const auto steps = work_order.steps();
 
       const auto step1 = steps[0];
-      CHECK(step1.id() == 31);
       CHECK(step1.process_id() == "pickup");
       CHECK(step1.name() ==
         "pickup item");
 
       const auto& step2 = steps[1];
-      CHECK(step2.id() == 32);
       CHECK(step2.process_id() == "place");
       CHECK(step2.name() == "place item");
 
       const auto& step3 = steps[2];
-      CHECK(step3.id() == 33);
       CHECK(step3.process_id() == "inspect");
       CHECK(step3.name() == "inspect item");
     };

diff --git a/nexus_demos/README.md b/nexus_demos/README.md
@@ -54,12 +54,12 @@ ros2 launch nexus_demos workcell.launch.py workcell_id:=workcell_2 ros_domain_id
 
 `place_on_conveyor` work order:
 ```bash
-ros2 action send_goal /system_orchestrator/execute_order nexus_orchestrator_msgs/action/ExecuteWorkOrder "{order: {id: '23', work_order: '$(cat config/place_on_conveyor.json)'}}"
+ros2 action send_goal /system_orchestrator/execute_order nexus_orchestrator_msgs/action/ExecuteWorkOrder "{order: {work_order_id: '23', work_order: '$(cat config/place_on_conveyor.json)'}}"
 ```
 
 `pick_from_conveyor` work order:
 ```bash
-ros2 action send_goal /system_orchestrator/execute_order nexus_orchestrator_msgs/action/ExecuteWorkOrder "{order: {id: '24', work_order: '$(cat config/pick_from_conveyor.json)'}}"
+ros2 action send_goal /system_orchestrator/execute_order nexus_orchestrator_msgs/action/ExecuteWorkOrder "{order: {work_order_id: '24', work_order: '$(cat config/pick_from_conveyor.json)'}}"
 ```
 
 ## Debugging

diff --git a/nexus_demos/config/pick_and_place.json b/nexus_demos/config/pick_and_place.json
@@ -1,5 +1,4 @@
 {
-  "number": "test-1",
   "workInstructionName": "Pick and Place",
   "item": {
     "SkuId": "productA",
@@ -10,12 +9,10 @@
   },
   "steps": [
     {
-      "id": 1.0,
       "processId": "place_on_conveyor",
       "name": "Pick and Place"
     },
     {
-      "id": 2.0,
       "processId": "pick_from_conveyor",
       "name": "Pick and Place"
     }

diff --git a/nexus_demos/config/pick_from_conveyor.json b/nexus_demos/config/pick_from_conveyor.json
@@ -1,6 +1,5 @@
 {
-  "number": "test-1",
-  "workInstructionName": "Pick and Place",
+  "workInstructionName": "Pick from conveyor",
   "item": {
     "SkuId": "productA",
     "description": "productA",
@@ -10,7 +9,6 @@
   },
   "steps": [
     {
-      "id": 1.0,
       "processId": "pick_from_conveyor",
       "name": "Pick and Place"
     }

diff --git a/nexus_demos/config/place_on_conveyor.json b/nexus_demos/config/place_on_conveyor.json
@@ -1,6 +1,5 @@
 {
-  "number": "test-2",
-  "workInstructionName": "Pick and Place",
+  "workInstructionName": "Place on conveyor",
   "item": {
     "SkuId": "productA",
     "description": "productA",
@@ -10,7 +9,6 @@
   },
   "steps": [
     {
-      "id": 2.0,
       "processId": "place_on_conveyor",
       "name": "Pick and Place"
     }

diff --git a/nexus_demos/config/workcell_task.yaml b/nexus_demos/config/workcell_task.yaml
@@ -3,7 +3,6 @@ task:
   type: "place_on_conveyor"
   payload: |
     {
-      "id": 2.0,
       "processId": "place_on_conveyor",
       "name": "Pick and Place",
       "item": {

diff --git a/nexus_demos/test_parallel_duplicated_wo.py b/nexus_demos/test_parallel_duplicated_wo.py
@@ -0,0 +1,90 @@
+# Copyright (C) 2025 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.
+
+import os
+import sys
+from typing import cast
+
+from nexus_orchestrator_msgs.action import ExecuteWorkOrder
+from nexus_test_case import NexusTestCase
+from managed_process import managed_process
+from rclpy.action import ActionClient
+from rclpy.action.client import ClientGoalHandle
+from ros_testcase import RosTestCase
+import subprocess
+
+
+class ParallelWoTest(NexusTestCase):
+    @RosTestCase.timeout(60)
+    async def asyncSetUp(self):
+        # todo(YV): Find a better fix to the problem below.
+        # zenoh-bridge was bumped to 0.72 as part of the upgrade to
+        # ROS 2 Iron. However with this upgrade, the bridge does not clearly
+        # terminate when a SIGINT is received leaving behind zombie bridge
+        # processes from previous test cases. As a result, workcell registration
+        # fails for this testcase due to multiple bridges remaining active.
+        # Hence we explicitly kill any zenoh processes before launching the test.
+        subprocess.Popen('pkill -9 -f zenoh', shell=True)
+
+        self.proc = managed_process(
+            ("ros2", "launch", "nexus_demos", "launch.py"),
+        )
+        self.proc.__enter__()
+        print("waiting for nodes to be ready...", file=sys.stderr)
+        self.wait_for_nodes("system_orchestrator")
+        await self.wait_for_lifecycle_active("system_orchestrator")
+
+        await self.wait_for_workcells("workcell_1", "workcell_2")
+        print("all workcells are ready")
+        await self.wait_for_transporters("transporter_node")
+        print("all transporters are ready")
+
+        # create action client to send work order
+        self.action_client = ActionClient(
+            self.node, ExecuteWorkOrder, "/system_orchestrator/execute_order"
+        )
+        self.action_client.wait_for_server()
+
+    def tearDown(self):
+        self.proc.__exit__(None, None, None)
+
+    @RosTestCase.timeout(180)  # 3min
+    async def test_reject_jobs_over_max(self):
+        """
+        New jobs should be rejected when the max number of jobs is already executing.
+        """
+        goal_msg = ExecuteWorkOrder.Goal()
+        goal_msg.order.work_order_id = "1"
+        with open(f"{os.path.dirname(__file__)}/config/pick_and_place.json") as f:
+            goal_msg.order.work_order = f.read()
+        goal_handle = cast(
+            ClientGoalHandle, await self.action_client.send_goal_async(goal_msg)
+        )
+        self.assertTrue(goal_handle.accepted)
+
+        goal_msg_2 = ExecuteWorkOrder.Goal()
+        goal_msg_2.order.work_order_id = "2"
+        with open(f"{os.path.dirname(__file__)}/config/pick_and_place.json") as f:
+            goal_msg_2.order.work_order = f.read()
+        goal_handle_2 = cast(
+            ClientGoalHandle, await self.action_client.send_goal_async(goal_msg_2)
+        )
+        self.assertTrue(goal_handle_2.accepted)
+
+        goal_msg_3 = goal_msg
+        goal_msg_3.order.work_order_id = "3"
+        goal_handle_3 = cast(
+            ClientGoalHandle, await self.action_client.send_goal_async(goal_msg_3)
+        )
+        self.assertFalse(goal_handle_3.accepted)
diff --git a/nexus_demos/test_parallel_wo.py b/nexus_demos/test_parallel_wo.py
@@ -65,7 +65,7 @@ async def test_reject_jobs_over_max(self):
         New jobs should be rejected when the max number of jobs is already executing.
         """
         goal_msg = ExecuteWorkOrder.Goal()
-        goal_msg.order.id = "1"
+        goal_msg.order.work_order_id = "1"
         with open(f"{os.path.dirname(__file__)}/config/place_on_conveyor.json") as f:
             goal_msg.order.work_order = f.read()
         goal_handle = cast(
@@ -74,7 +74,7 @@ async def test_reject_jobs_over_max(self):
         self.assertTrue(goal_handle.accepted)
 
         goal_msg_2 = ExecuteWorkOrder.Goal()
-        goal_msg_2.order.id = "2"
+        goal_msg_2.order.work_order_id = "2"
         with open(f"{os.path.dirname(__file__)}/config/pick_from_conveyor.json") as f:
             goal_msg_2.order.work_order = f.read()
         goal_handle_2 = cast(
@@ -83,7 +83,7 @@ async def test_reject_jobs_over_max(self):
         self.assertTrue(goal_handle_2.accepted)
 
         goal_msg_3 = goal_msg
-        goal_msg_3.order.id = "3"
+        goal_msg_3.order.work_order_id = "3"
         goal_handle_3 = cast(
             ClientGoalHandle, await self.action_client.send_goal_async(goal_msg_3)
         )

diff --git a/nexus_demos/test_pick_and_place.py b/nexus_demos/test_pick_and_place.py
@@ -67,6 +67,7 @@ def tearDown(self):
     async def test_pick_and_place_wo(self):
         self.action_client.wait_for_server()
         goal_msg = ExecuteWorkOrder.Goal()
+        goal_msg.order.work_order_id = "1"
         with open(f"{os.path.dirname(__file__)}/config/pick_from_conveyor.json") as f:
             goal_msg.order.work_order = f.read()
         feedbacks: list[ExecuteWorkOrder.Feedback] = []
@@ -93,7 +94,7 @@ def on_fb(msg):
             self.assertEqual(len(msg.task_states), 1)
             state: TaskState = msg.task_states[0]  # type: ignore
             self.assertEqual(state.workcell_id, "workcell_2")
-            self.assertEqual(state.task_id, "1")
+            self.assertEqual(state.task_id,"1/pick_from_conveyor/0")
 
         state: TaskState = feedbacks[-1].task_states[0]  # type: ignore
         self.assertEqual(state.status, TaskState.STATUS_FINISHED)
diff --git a/nexus_demos/test_pick_and_place_rmf.py b/nexus_demos/test_pick_and_place_rmf.py
@@ -74,6 +74,7 @@ def tearDown(self):
     async def test_pick_and_place_wo(self):
         self.action_client.wait_for_server()
         goal_msg = ExecuteWorkOrder.Goal()
+        goal_msg.order.work_order_id = "1"
         with open(f"{os.path.dirname(__file__)}/config/pick_and_place.json") as f:
             goal_msg.order.work_order = f.read()
         feedbacks: list[ExecuteWorkOrder.Feedback] = []
@@ -101,10 +102,10 @@ def on_fb(msg):
             self.assertEqual(len(msg.task_states), 3)
             state: TaskState = msg.task_states[1]  # type: ignore
             self.assertEqual(state.workcell_id, "workcell_1")
-            self.assertEqual(state.task_id, "1")
+            self.assertEqual(state.task_id, "1/place_on_conveyor/0")
             state: TaskState = msg.task_states[2]  # type: ignore
             self.assertEqual(state.workcell_id, "workcell_2")
-            self.assertEqual(state.task_id, "2")
+            self.assertEqual(state.task_id, "1/pick_from_conveyor/1")
 
         state: TaskState = feedbacks[-1].task_states[0]  # type: ignore
         self.assertEqual(state.status, TaskState.STATUS_FINISHED)

diff --git a/nexus_msgs/nexus_orchestrator_msgs/msg/WorkOrder.msg b/nexus_msgs/nexus_orchestrator_msgs/msg/WorkOrder.msg
@@ -1,5 +1,5 @@
-# id of the work order
-string id
+# A unique ID for this work order
+string work_order_id
 
 # yaml representation of the work order
 string work_order
diff --git a/nexus_msgs/nexus_orchestrator_msgs/msg/WorkcellState.msg b/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

diff --git a/nexus_msgs/nexus_orchestrator_msgs/msg/WorkcellTask.msg b/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

diff --git a/nexus_system_orchestrator/src/assign_transporter_workcell.cpp b/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;