From 2ddcafafd72c9565c97a599819796d4505dc1ddc Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 6 Jan 2025 15:33:34 +0800 Subject: [PATCH] Split main / rmf main and create new integration test Signed-off-by: Luca Della Vedova --- .../config/pick_and_place_rmf.json | 23 ++++ .../config/system_bts/main.xml | 28 ++--- .../config/system_bts/main_rmf.xml | 19 +++ .../config/system_bts/pick_and_place.xml | 14 ++- .../config/system_bts/pick_and_place_rmf.xml | 13 +++ .../config/workcell_1_bts/place_on_amr.xml | 29 +++++ .../workcell_1_bts/place_on_conveyor.xml | 1 + .../config/workcell_2_bts/pick_from_amr.xml | 33 ++++++ .../workcell_2_bts/pick_from_conveyor.xml | 1 + .../launch/control_center.launch.py | 1 + nexus_integration_tests/test_parallel_wo.py | 2 +- .../test_pick_and_place.py | 9 +- .../test_pick_and_place_rmf.py | 109 ++++++++++++++++++ 13 files changed, 256 insertions(+), 26 deletions(-) create mode 100644 nexus_integration_tests/config/pick_and_place_rmf.json create mode 100644 nexus_integration_tests/config/system_bts/main_rmf.xml create mode 100644 nexus_integration_tests/config/system_bts/pick_and_place_rmf.xml create mode 100644 nexus_integration_tests/config/workcell_1_bts/place_on_amr.xml create mode 100644 nexus_integration_tests/config/workcell_2_bts/pick_from_amr.xml create mode 100644 nexus_integration_tests/test_pick_and_place_rmf.py diff --git a/nexus_integration_tests/config/pick_and_place_rmf.json b/nexus_integration_tests/config/pick_and_place_rmf.json new file mode 100644 index 0000000..8d71b62 --- /dev/null +++ b/nexus_integration_tests/config/pick_and_place_rmf.json @@ -0,0 +1,23 @@ +{ + "number": "test-1", + "workInstructionName": "Pick and Place", + "item": { + "SkuId": "productA", + "description": "productA", + "unit": "dummy_unit", + "quantity": 1.0, + "quantityPerPallet": 1.0 + }, + "steps": [ + { + "id": 1.0, + "processId": "place_on_amr", + "name": "Pick and Place" + }, + { + "id": 2.0, + "processId": "pick_from_amr", + "name": "Pick and Place" + } + ] +} diff --git a/nexus_integration_tests/config/system_bts/main.xml b/nexus_integration_tests/config/system_bts/main.xml index 3588eef..3f96eaf 100644 --- a/nexus_integration_tests/config/system_bts/main.xml +++ b/nexus_integration_tests/config/system_bts/main.xml @@ -1,19 +1,21 @@ - - - - - - - - - - - - - + + + + + + + + + + + + + + + diff --git a/nexus_integration_tests/config/system_bts/main_rmf.xml b/nexus_integration_tests/config/system_bts/main_rmf.xml new file mode 100644 index 0000000..3588eef --- /dev/null +++ b/nexus_integration_tests/config/system_bts/main_rmf.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + 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 c413801..07a7e78 100644 --- a/nexus_integration_tests/config/system_bts/pick_and_place.xml +++ b/nexus_integration_tests/config/system_bts/pick_and_place.xml @@ -1,13 +1,15 @@ - - - + + + + + + + - - - + diff --git a/nexus_integration_tests/config/system_bts/pick_and_place_rmf.xml b/nexus_integration_tests/config/system_bts/pick_and_place_rmf.xml new file mode 100644 index 0000000..c413801 --- /dev/null +++ b/nexus_integration_tests/config/system_bts/pick_and_place_rmf.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/nexus_integration_tests/config/workcell_1_bts/place_on_amr.xml b/nexus_integration_tests/config/workcell_1_bts/place_on_amr.xml new file mode 100644 index 0000000..a98c761 --- /dev/null +++ b/nexus_integration_tests/config/workcell_1_bts/place_on_amr.xml @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nexus_integration_tests/config/workcell_1_bts/place_on_conveyor.xml b/nexus_integration_tests/config/workcell_1_bts/place_on_conveyor.xml index a98c761..7315fa6 100644 --- a/nexus_integration_tests/config/workcell_1_bts/place_on_conveyor.xml +++ b/nexus_integration_tests/config/workcell_1_bts/place_on_conveyor.xml @@ -17,6 +17,7 @@ + diff --git a/nexus_integration_tests/config/workcell_2_bts/pick_from_amr.xml b/nexus_integration_tests/config/workcell_2_bts/pick_from_amr.xml new file mode 100644 index 0000000..d5e2872 --- /dev/null +++ b/nexus_integration_tests/config/workcell_2_bts/pick_from_amr.xml @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nexus_integration_tests/config/workcell_2_bts/pick_from_conveyor.xml b/nexus_integration_tests/config/workcell_2_bts/pick_from_conveyor.xml index d5e2872..4e2e2f8 100644 --- a/nexus_integration_tests/config/workcell_2_bts/pick_from_conveyor.xml +++ b/nexus_integration_tests/config/workcell_2_bts/pick_from_conveyor.xml @@ -2,6 +2,7 @@ + diff --git a/nexus_integration_tests/launch/control_center.launch.py b/nexus_integration_tests/launch/control_center.launch.py index 709da15..c22a013 100644 --- a/nexus_integration_tests/launch/control_center.launch.py +++ b/nexus_integration_tests/launch/control_center.launch.py @@ -146,6 +146,7 @@ def launch_setup(context, *args, **kwargs): "remap_task_types": """{ pick_and_place: [place_on_conveyor, pick_from_conveyor], + pick_and_place_rmf: [place_on_amr, pick_from_amr], }""", "main_bt_filename": main_bt_filename, "max_jobs": 2, diff --git a/nexus_integration_tests/test_parallel_wo.py b/nexus_integration_tests/test_parallel_wo.py index cad860f..48d3d8c 100644 --- a/nexus_integration_tests/test_parallel_wo.py +++ b/nexus_integration_tests/test_parallel_wo.py @@ -38,7 +38,7 @@ async def asyncSetUp(self): subprocess.Popen('pkill -9 -f zenoh', shell=True) self.proc = managed_process( - ("ros2", "launch", "nexus_integration_tests", "office.launch.xml", "sim_update_rate:=10000"), + ("ros2", "launch", "nexus_integration_tests", "launch.py"), ) self.proc.__enter__() print("waiting for nodes to be ready...", file=sys.stderr) diff --git a/nexus_integration_tests/test_pick_and_place.py b/nexus_integration_tests/test_pick_and_place.py index 7196900..6369320 100644 --- a/nexus_integration_tests/test_pick_and_place.py +++ b/nexus_integration_tests/test_pick_and_place.py @@ -40,7 +40,7 @@ async def asyncSetUp(self): subprocess.Popen('pkill -9 -f zenoh', shell=True) self.proc = managed_process( - ("ros2", "launch", "nexus_integration_tests", "office.launch.xml", "sim_update_rate:=10000"), + ("ros2", "launch", "nexus_integration_tests", "launch.py"), ) self.proc.__enter__() print("waiting for nodes to be ready...", file=sys.stderr) @@ -51,8 +51,6 @@ async def asyncSetUp(self): print("all workcells are ready") await self.wait_for_transporters("transporter_node") print("all transporters are ready") - await self.wait_for_robot_state() - print("AMRs are ready") # give some time for discovery to happen await self.ros_sleep(5) @@ -92,9 +90,8 @@ def on_fb(msg): # high load so we only check the last feedback as a workaround. self.assertGreater(len(feedbacks), 0) for msg in feedbacks: - # The first task is transportation - self.assertEqual(len(msg.task_states), 2) - state: TaskState = msg.task_states[1] # type: ignore + 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") diff --git a/nexus_integration_tests/test_pick_and_place_rmf.py b/nexus_integration_tests/test_pick_and_place_rmf.py new file mode 100644 index 0000000..45fc6b8 --- /dev/null +++ b/nexus_integration_tests/test_pick_and_place_rmf.py @@ -0,0 +1,109 @@ +# Copyright (C) 2022 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. + +import os +import sys +from typing import cast + +from action_msgs.msg import GoalStatus +from managed_process import managed_process +from nexus_orchestrator_msgs.action import ExecuteWorkOrder +from nexus_orchestrator_msgs.msg import TaskState +from nexus_test_case import NexusTestCase +from rclpy import Future +from rclpy.action import ActionClient +from rclpy.action.client import ClientGoalHandle, GoalStatus +from ros_testcase import RosTestCase +import subprocess + +class PickAndPlaceTest(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_integration_tests", "office.launch.xml", "sim_update_rate:=10000", "main_bt_filename:=main_rmf.xml"), + ) + 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", "rmf_nexus_transporter") + print("all workcells are ready") + await self.wait_for_transporters("transporter_node") + print("all transporters are ready") + await self.wait_for_robot_state() + print("AMRs are ready") + + # give some time for discovery to happen + await self.ros_sleep(5) + + # create action client to send work order + self.action_client = ActionClient( + self.node, ExecuteWorkOrder, "/system_orchestrator/execute_order" + ) + + def tearDown(self): + self.proc.__exit__(None, None, None) + + @RosTestCase.timeout(600) # 10min + async def test_pick_and_place_wo(self): + self.action_client.wait_for_server() + goal_msg = ExecuteWorkOrder.Goal() + with open(f"{os.path.dirname(__file__)}/config/pick_and_place_rmf.json") as f: + goal_msg.order.work_order = f.read() + feedbacks: list[ExecuteWorkOrder.Feedback] = [] + fb_fut = Future() + + def on_fb(msg): + feedbacks.append(msg.feedback) + if len(feedbacks) >= 5: + fb_fut.set_result(None) + + goal_handle = cast( + ClientGoalHandle, await self.action_client.send_goal_async(goal_msg, on_fb) + ) + self.assertTrue(goal_handle.accepted) + + results = await goal_handle.get_result_async() + self.assertEqual(results.status, GoalStatus.STATUS_SUCCEEDED) + + # check that we receive the correct feedbacks + # FIXME(koonpeng): First few feedbacks are sometimes missed when the system in under + # high load so we only check the last feedback as a workaround. + self.assertGreater(len(feedbacks), 0) + for msg in feedbacks: + # The first task is transportation + 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") + state: TaskState = msg.task_states[2] # type: ignore + self.assertEqual(state.workcell_id, "workcell_2") + self.assertEqual(state.task_id, "2") + + state: TaskState = feedbacks[-1].task_states[0] # type: ignore + self.assertEqual(state.status, TaskState.STATUS_FINISHED) + state: TaskState = feedbacks[-1].task_states[1] # type: ignore + self.assertEqual(state.status, TaskState.STATUS_FINISHED) + state: TaskState = feedbacks[-1].task_states[2] # type: ignore + self.assertEqual(state.status, TaskState.STATUS_FINISHED)