Split main / rmf main and create new integration test

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

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"
+    }
+  ]
+}

nexus_integration_tests/config/system_bts/main.xml

@@ -1,19 +1,21 @@
 <root main_tree_to_execute="main">
   <BehaviorTree ID="main">
     <ReactiveSequence>
-      <!--IsPauseTriggered paused="{paused}"/-->
-      <!--PausableSequence pause="{paused}"-->
-        <!-- Iterates over all task assignments and dispatches an AMR -->
-        <Sequence>
-          <DispatchTransporter name="bid_amr_transporter" result="{amr_transporter}" transport_task="{transport_task}"/>
-          <Parallel success_threshold="-1">
-            <WorkcellRequest name="amr_request" workcell="{amr_transporter}" task="{transport_task}"/>
-            <ForEachTask task="{task}" workcell="{workcell}">
-              <ExecuteTask task="{task}" workcell="{workcell}" transport_task="{transport_task}"/>
-            </ForEachTask>
-          </Parallel>
-        </Sequence>
-      <!--/PausableSequence-->
+      <IsPauseTriggered paused="{paused}"/>
+      <Fallback>
+        <PausableSequence pause="{paused}">
+          <ForEachTask task="{task}" workcell="{workcell}">
+            <ExecuteTask task="{task}" workcell="{workcell}"/>
+          </ForEachTask>
+          <BidTransporter name="bid_transporter" destination="unloading" result="{transporter}"/>
+          <TransporterRequest name="transporter_request" transporter="{transporter}" destination="unloading"/>
+        </PausableSequence>
+        <PausableSequence pause="{paused}">
+          <BidTransporter name="bid_transporter" destination="unloading" result="{transporter}"/>
+          <TransporterRequest name="transporter_request" transporter="{transporter}" destination="unloading"/>
+          <AlwaysFailure/>
+        </PausableSequence>
+      </Fallback>
     </ReactiveSequence>
   </BehaviorTree>
 </root>

nexus_integration_tests/config/system_bts/main_rmf.xml

@@ -0,0 +1,19 @@
+<root main_tree_to_execute="main">
+  <BehaviorTree ID="main">
+    <ReactiveSequence>
+      <!--IsPauseTriggered paused="{paused}"/-->
+      <!--PausableSequence pause="{paused}"-->
+        <!-- Iterates over all task assignments and dispatches an AMR -->
+        <Sequence>
+          <DispatchTransporter name="bid_amr_transporter" result="{amr_transporter}" transport_task="{transport_task}"/>
+          <Parallel success_threshold="-1">
+            <WorkcellRequest name="amr_request" workcell="{amr_transporter}" task="{transport_task}"/>
+            <ForEachTask task="{task}" workcell="{workcell}">
+              <ExecuteTask task="{task}" workcell="{workcell}" transport_task="{transport_task}"/>
+            </ForEachTask>
+          </Parallel>
+        </Sequence>
+      <!--/PausableSequence-->
+    </ReactiveSequence>
+  </BehaviorTree>
+</root>

nexus_integration_tests/config/system_bts/pick_and_place.xml

@@ -1,13 +1,15 @@
 <root main_tree_to_execute="pick_and_place">
   <BehaviorTree ID="pick_and_place">
     <ReactiveSequence>
-      <!--IsPauseTriggered paused="{paused}"/-->
-      <Sequence>
-        <WaitForSignal name="wait_for_amr" signal="{workcell}" clear="true"/>
+      <IsPauseTriggered paused="{paused}"/>
+      <Parallel success_threshold="-1">
+        <Sequence>
+          <BidTransporter name="bid_transporterasdasd" destination="{workcell}" result="{transporter}"/>
+          <TransporterRequest name="transporter_request" transporter="{transporter}" destination="{workcell}"/>
+          <SendSignal task="{task}" signal="transporter_done"/>
+        </Sequence>
         <WorkcellRequest name="workcell_request" workcell="{workcell}" task="{task}"/>
-        <!-- The transporter will listen and send an appropriate dispenser result for RMF or noop for conveyor belt-->
-        <SendSignal task="{transport_task}" signal="{workcell}"/>
-      </Sequence>
+      </Parallel>
     </ReactiveSequence>
   </BehaviorTree>
 </root>

nexus_integration_tests/config/system_bts/pick_and_place_rmf.xml

@@ -0,0 +1,13 @@
+<root main_tree_to_execute="pick_and_place">
+  <BehaviorTree ID="pick_and_place">
+    <ReactiveSequence>
+      <!--IsPauseTriggered paused="{paused}"/-->
+      <Sequence>
+        <WaitForSignal name="wait_for_amr" signal="{workcell}" clear="true"/>
+        <WorkcellRequest name="workcell_request" workcell="{workcell}" task="{task}"/>
+        <!-- The transporter will listen and send an appropriate dispenser result for RMF or noop for conveyor belt-->
+        <SendSignal task="{transport_task}" signal="{workcell}"/>
+      </Sequence>
+    </ReactiveSequence>
+  </BehaviorTree>
+</root>

nexus_integration_tests/config/workcell_1_bts/place_on_amr.xml

@@ -0,0 +1,29 @@
+<?xml version='1.0'?>
+<root main_tree_to_execute="PickAndPlace">
+  <BehaviorTree ID="PickAndPlace">
+    <Sequence name="root_sequence">
+      <dispense_item.DispenseItem name="dispense_item" item="productA" />
+
+      <ApplyTransform base_pose="pickup_pose" x="0" y="0" z="-0.02" qx="0" qy="0" qz="0" qw="1" local="true" result="{pickup_top_goal}" />
+      <plan_motion.PlanMotion name="plan_to_pickup_top" robot_name="abb_irb910sc" goal="{pickup_top_goal}" scale_speed="0.5" cartesian="true" result="{pickup_top_traj}" />
+      <GetJointConstraints trajectory="{pickup_top_traj}" joint_constraints="{joint_constraints}" />
+      <execute_trajectory.ExecuteTrajectory name="execute_pickup_top" trajectory="{pickup_top_traj}" />
+
+      <plan_motion.PlanMotion name="plan_to_pickup" robot_name="abb_irb910sc" goal="pickup_pose" scale_speed="0.5" cartesian="true" start_constraints="{joint_constraints}" result="{pickup_traj}" />
+      <GetJointConstraints trajectory="{pickup_traj}" joint_constraints="{joint_constraints}" />
+      <execute_trajectory.ExecuteTrajectory name="execute_pickup" trajectory="{pickup_traj}" />
+      <gripper.GripperControl name="pickup_product" gripper="workcell_1_mock_gripper" position="0.0" max_effort="1.0" />
+
+      <plan_motion.PlanMotion name="plan_to_dropoff" robot_name="abb_irb910sc" goal="dropoff_pose" scale_speed="0.5" cartesian="true" start_constraints="{joint_constraints}" result="{dropoff_traj}" />
+      <GetJointConstraints trajectory="{dropoff_traj}" joint_constraints="{joint_constraints}" />
+
+      <execute_trajectory.ExecuteTrajectory name="execute_dropoff" trajectory="{dropoff_traj}" />
+      <gripper.GripperControl name="dropoff_product" gripper="workcell_1_mock_gripper" position="1.0" max_effort="0.0" />
+
+      <plan_motion.PlanMotion name="plan_to_home" robot_name="abb_irb910sc" goal="home" scale_speed="0.5" cartesian="true" start_constraints="{joint_constraints}" result="{home_traj}" />
+      <execute_trajectory.ExecuteTrajectory name="execute_home" trajectory="{home_traj}" />
+
+
+    </Sequence>
+  </BehaviorTree>
+</root>

nexus_integration_tests/config/workcell_1_bts/place_on_conveyor.xml

@@ -17,6 +17,7 @@
       <plan_motion.PlanMotion name="plan_to_dropoff" robot_name="abb_irb910sc" goal="dropoff_pose" scale_speed="0.5" cartesian="true" start_constraints="{joint_constraints}" result="{dropoff_traj}" />
       <GetJointConstraints trajectory="{dropoff_traj}" joint_constraints="{joint_constraints}" />
 
+      <WaitForSignal name="wait_for_pallet" signal="transporter_done" clear="true"/>
       <execute_trajectory.ExecuteTrajectory name="execute_dropoff" trajectory="{dropoff_traj}" />
       <gripper.GripperControl name="dropoff_product" gripper="workcell_1_mock_gripper" position="1.0" max_effort="0.0" />
 

nexus_integration_tests/config/workcell_2_bts/pick_from_amr.xml

@@ -0,0 +1,33 @@
+<?xml version='1.0'?>
+<root main_tree_to_execute="PickAndPlace">
+  <BehaviorTree ID="PickAndPlace">
+    <Sequence name="root_sequence">
+      <StringToVector string="productA," delimiter="," strings="{items}"/>
+      <detection.DetectAllItems name="detect_items" detector="workcell_2_mock_product_detector" items="{items}" result="{all_products}" />
+      <detection.GetDetection detections="{all_products}" idx="0" result="{current_product_detection}" />
+      <detection.GetDetectionPose detection="{current_product_detection}" result="{current_product_pose}" />
+
+      <ApplyTransform base_pose="{current_product_pose}" x="0" y="0" z="-0.02" qx="0" qy="0" qz="0" qw="1" local="true" result="{pickup_top_goal}" />
+      <plan_motion.PlanMotion name="plan_to_pickup_top" robot_name="abb_irb1300" goal="{pickup_top_goal}" scale_speed="0.5" result="{pickup_top_traj}" />
+      <GetJointConstraints trajectory="{pickup_top_traj}" joint_constraints="{joint_constraints}" />
+      <execute_trajectory.ExecuteTrajectory name="execute_pickup" trajectory="{pickup_top_traj}" />
+
+      <plan_motion.PlanMotion name="plan_to_product" robot_name="abb_irb1300" goal="{current_product_pose}" scale_speed="0.5" cartesian="true" start_constraints="{joint_constraints}" result="{pickup_traj}" />
+      <GetJointConstraints trajectory="{pickup_traj}" joint_constraints="{joint_constraints}" />
+      <execute_trajectory.ExecuteTrajectory name="execute_pickup" trajectory="{pickup_traj}" />
+      <gripper.GripperControl name="pickup_product" gripper="workcell_2_mock_gripper" position="0.0" max_effort="1.0" />
+
+      <ApplyTransform base_pose="dropoff_pose" x="0" y="0" z="-0.02" qx="0" qy="0" qz="0" qw="1" result="{dropoff_top_goal}" />
+      <plan_motion.PlanMotion name="plan_to_dropoff_top" robot_name="abb_irb1300" goal="{dropoff_top_goal}" scale_speed="0.5" result="{dropoff_top_traj}" />
+      <GetJointConstraints trajectory="{dropoff_top_traj}" joint_constraints="{joint_constraints}" />
+      <execute_trajectory.ExecuteTrajectory name="execute_dropoff_top" trajectory="{dropoff_top_traj}" />
+
+      <plan_motion.PlanMotion name="plan_to_dropoff" robot_name="abb_irb1300" goal="dropoff_pose" scale_speed="0.5" cartesian="true" start_constraints="{joint_constraints}" result="{dropoff_traj}" />
+      <GetJointConstraints trajectory="{dropoff_traj}" joint_constraints="{joint_constraints}" />
+      <gripper.GripperControl name="pickup_product" gripper="workcell_2_mock_gripper" position="1.0" max_effort="0.0" />
+
+      <plan_motion.PlanMotion name="plan_to_home" robot_name="abb_irb1300" goal="home" scale_speed="0.5" start_constraints="{joint_constraints}" result="{home_traj}" />
+      <execute_trajectory.ExecuteTrajectory name="execute_home" trajectory="{home_traj}" />
+    </Sequence>
+  </BehaviorTree>
+</root>

nexus_integration_tests/config/workcell_2_bts/pick_from_conveyor.xml

@@ -2,6 +2,7 @@
 <root main_tree_to_execute="PickAndPlace">
   <BehaviorTree ID="PickAndPlace">
     <Sequence name="root_sequence">
+      <WaitForSignal name="wait_for_pallet" signal="transporter_done" clear="true"/>
       <StringToVector string="productA," delimiter="," strings="{items}"/>
       <detection.DetectAllItems name="detect_items" detector="workcell_2_mock_product_detector" items="{items}" result="{all_products}" />
       <detection.GetDetection detections="{all_products}" idx="0" result="{current_product_detection}" />

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,

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)

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")
 

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)