Add integration test for lab_sim

Signed-off-by: Paul Gesel <[email protected]>

Author: pac48

src/lab_sim/CMakeLists.txt

@@ -41,8 +41,13 @@ install(
 )
 
 if(BUILD_TESTING)
+  find_package(ament_cmake_pytest REQUIRED)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
+  ament_add_pytest_test(
+          test_objective_server_node_integration test/test_core_objectives.py
+          TIMEOUT 600
+          ENV STUDIO_CONFIG_PACKAGE=lab_sim STUDIO_HOST_USER_WORKSPACE=${CMAKE_SOURCE_DIR})
 endif()
 
 ament_package()

src/lab_sim/objectives/apriltag_detection_config.yaml

@@ -0,0 +1,9 @@
+DetectAprilTags:
+  apriltag_family_name: 36h11
+  apriltag_size: 0.04
+  max_hamming: 0
+  n_threads: 1
+  quad_decimate: 2
+  quad_sigma: 0.0
+  refine_edges: true
+  z_up: false

src/lab_sim/objectives/force_relaxation.xml

@@ -26,6 +26,8 @@
             publish_rate="50"
             twist_stamped="{zero_twist}"
             wrench_stamped="{zero_wrench}"
+            velocity_controlled_axes="0;0;0;0;0;0"
+            velocity_force_controller_command_topic="/velocity_force_controller/command"
           />
         </Decorator>
       </Decorator>
@@ -39,19 +41,21 @@
             velocity_controlled_axes="1;1;1;0;0;0"
             wrench_stamped="{zero_wrench}"
             twist_stamped="{zero_twist}"
+            velocity_force_controller_command_topic="/velocity_force_controller/command"
           />
         </Decorator>
       </Decorator>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Force Relaxation">
-      <input_port name="wrench_gain" default="0.01" />
-      <input_port name="relaxation_time_ms" default="500" />
-      <input_port name="tip_link" default="grasp_link" />
       <MetadataFields>
         <Metadata subcategory="Motion - Controls" />
+        <Metadata runnable="false" />
       </MetadataFields>
+      <input_port name="relaxation_time_ms" default="500" />
+      <input_port name="tip_link" default="grasp_link" />
+      <input_port name="wrench_gain" default="0.01" />
     </SubTree>
   </TreeNodesModel>
 </root>

src/lab_sim/objectives/grasp_planning.xml

@@ -67,7 +67,7 @@
         <Action
           ID="SetupMTCConnectWithTrajectory"
           constraints="{constraints}"
-          planner_interface="moveit_pro"
+          planner_interface="pro_rrt"
           planning_group_name="manipulator"
           task="{pick_object_task}"
         />

src/lab_sim/objectives/grasp_pose_tuning_with_april_tag.xml

@@ -1,4 +1,4 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version="1.0" encoding="UTF-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Grasp Pose Tuning With April Tag">
   <!--//////////-->
   <BehaviorTree
@@ -94,7 +94,7 @@
         source_to_destination_pose="{relative_pose}"
       />
       <Action
-        ID="SavePoseToYaml"
+        ID="SavePoseStampedToYaml"
         message="{relative_pose}"
         yaml_filename="relative_grasp_pose"
         namespace="RelativePose"

src/lab_sim/objectives/grasp_pose_using_yaml.xml

@@ -95,6 +95,8 @@
         ID="Pick from Pose"
         _collapsed="true"
         grasp_poses="{grasp_poses}"
+        approach_distance="0.1"
+        retract_xyz="0;0;-0.1"
       />
     </Control>
   </BehaviorTree>

src/lab_sim/objectives/load_mesh_as_green_poincloud.xml

@@ -32,6 +32,7 @@
       <input_port name="initial_pose" default="{stamped_pose}" />
       <MetadataFields>
         <Metadata subcategory="Perception - 3D Point Cloud" />
+        <Metadata runnable="false" />
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>

src/lab_sim/objectives/load_mesh_as_red_pointcloud.xml

@@ -32,6 +32,7 @@
       <input_port name="initial_pose" default="{stamped_pose}" />
       <MetadataFields>
         <Metadata subcategory="Perception - 3D Point Cloud" />
+        <Metadata runnable="false" />
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>

src/lab_sim/objectives/plan_move_to_pose.xml

@@ -11,6 +11,7 @@
         task_id="move_to_pose"
         controller_names="/joint_trajectory_controller;/robotiq_gripper_controller"
         task="{move_to_pose_task}"
+        trajectory_monitoring="false"
       />
       <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}" />
       <Action
@@ -19,6 +20,15 @@
         planning_group_name="manipulator"
         target_pose="{target_pose}"
         task="{move_to_pose_task}"
+        acceleration_scale_factor="1.000000"
+        keep_orientation="false"
+        keep_orientation_link_names="grasp_link"
+        keep_orientation_tolerance="0.100000"
+        link_padding="0.000000"
+        max_iterations="5000"
+        monitored_stage="current state"
+        trajectory_sampling_rate="100"
+        velocity_scale_factor="1.000000"
       />
       <Action
         ID="PlanMTCTask"
@@ -29,14 +39,15 @@
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Plan Move To Pose">
-      <input_port name="target_pose" default="{target_pose}" />
+      <MetadataFields>
+        <Metadata subcategory="Motion - Planning" />
+        <Metadata runnable="false" />
+      </MetadataFields>
       <output_port
         name="move_to_pose_solution"
         default="{move_to_pose_solution}"
       />
-      <MetadataFields>
-        <Metadata subcategory="Motion - Planning" />
-      </MetadataFields>
+      <input_port name="target_pose" default="{target_pose}" />
     </SubTree>
   </TreeNodesModel>
 </root>

src/lab_sim/objectives/plan_to_pose.xml

@@ -1,3 +1,4 @@
+<?xml version="1.0" encoding="UTF-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Add Poses to MTC Task">
   <!--//////////-->
   <BehaviorTree
@@ -10,10 +11,11 @@
         ID="InitializeMTCTask"
         controller_names="/joint_trajectory_controller;/robotiq_gripper_controller"
         task="{mtc_task}"
+        trajectory_monitoring="false"
       />
-      <!-- Start the MTC task from the robot's current pose -->
+      <!--Start the MTC task from the robot's current pose-->
       <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
-      <!-- Use a script with a pose counter so we can give a unique name to each marker (allowing multiple poses to be visualized) -->
+      <!--Use a script with a pose counter so we can give a unique name to each marker (allowing multiple poses to be visualized)-->
       <Action ID="Script" code="pose_ctr := 0" />
       <Decorator
         ID="ForEach"
@@ -22,11 +24,13 @@
         index="{index}"
       >
         <Control ID="Sequence">
-          <!-- Visualize and add each pose to the vector -->
+          <!--Visualize and add each pose to the vector-->
           <Action
             ID="VisualizePose"
             marker_name="{pose_ctr}"
             pose="{target_pose}"
+            marker_lifetime="0.000000"
+            marker_size="0.100000"
           />
           <Action ID="Script" code="pose_ctr += 1" />
           <Action
@@ -35,24 +39,34 @@
             planning_group_name="{planning_group_name}"
             target_pose="{target_pose}"
             task="{mtc_task}"
+            acceleration_scale_factor="1.000000"
+            keep_orientation="false"
+            keep_orientation_link_names="grasp_link"
+            keep_orientation_tolerance="0.100000"
+            link_padding="0.000000"
+            max_iterations="5000"
+            monitored_stage="current state"
+            trajectory_sampling_rate="100"
+            velocity_scale_factor="1.000000"
           />
         </Control>
       </Decorator>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Add Poses to MTC Task">
-      <input_port name="target_poses" default="{target_poses}" />
+      <MetadataFields>
+        <Metadata subcategory="Motion - Task Planning" />
+        <Metadata runnable="false" />
+      </MetadataFields>
       <input_port
         name="controller_names"
         default="/joint_trajectory_controller;/robotiq_gripper_controller"
       />
-      <input_port name="planning_group_name" default="manipulator" />
       <input_port name="ik_frame" default="grasp_link" />
       <inout_port name="mtc_task" default="{mtc_task}" />
-      <MetadataFields>
-        <Metadata subcategory="Motion - Task Planning" />
-      </MetadataFields>
+      <input_port name="planning_group_name" default="manipulator" />
+      <input_port name="target_poses" default="{target_poses}" />
     </SubTree>
   </TreeNodesModel>
 </root>

src/lab_sim/test/test_core_objectives.py

@@ -0,0 +1,49 @@
+import time
+
+import pytest
+from moveit_pro_test_utils.objective_test_fixture import (
+    ExecuteObjectiveResource,
+    execute_objective_resource as execute_objective_resource,
+    get_objective_pytest_params,
+    run_objective,
+)
+
+cancel_objectives = {
+    "3 Waypoints Pick and Place",
+    "Cycle Between Waypoints",
+    "Grasp Object from Text Prompt",
+    "Grasp Planning",
+    "Grasp Pose Tuning With April Tag",
+    "Grasp Pose Using Yaml",
+    "Joint Diagnostic",
+    "Loop Detect AprilTag",
+    "Pick And Place Example",
+    "Pick April Tag Labeled Object With Approval",
+    "Place Object",
+    "Plan and Save Trajectory",
+    "Record and Replay Scanning Motion",
+}
+skip_objectives = {
+    "Grasp Planning",
+    "Joint Diagnostic",
+    "ML Segment Image from Text Prompt",
+    "ML Segment Point Cloud from Clicked Point",
+    "Pick up Cube",
+    "Place Object",
+    "Teleoperate",
+}
+
+
+@pytest.mark.parametrize(
+    "objective_id, should_cancel",
+    get_objective_pytest_params("lab_sim", cancel_objectives, skip_objectives),
+)
+def test_all_objectives(
+    objective_id: str,
+    should_cancel: bool,
+    execute_objective_resource: ExecuteObjectiveResource,
+):
+    if objective_id == "Close Gripper":
+        time.sleep(60*2)
+
+    run_objective(objective_id, should_cancel, execute_objective_resource)