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/test/test_core_objectives.py

@@ -0,0 +1,46 @@
+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,
+):
+    run_objective(objective_id, should_cancel, execute_objective_resource)