From 7e4e0c6b73f710b0d616b1e4418e3cc2c5f87a6f Mon Sep 17 00:00:00 2001
From: Paul Gesel <paul.gesel@picknik.ai>
Date: Tue, 5 Nov 2024 10:03:41 -0700
Subject: [PATCH] add move_group_api.test.py back

Signed-off-by: Paul Gesel <paul.gesel@picknik.ai>
---
 .../tests/launch/move_group_api.test.py       | 137 ++++++++++++++++++
 1 file changed, 137 insertions(+)
 create mode 100644 moveit_ros/tests/launch/move_group_api.test.py

diff --git a/moveit_ros/tests/launch/move_group_api.test.py b/moveit_ros/tests/launch/move_group_api.test.py
new file mode 100644
index 0000000000..4d66d6f018
--- /dev/null
+++ b/moveit_ros/tests/launch/move_group_api.test.py
@@ -0,0 +1,137 @@
+import os
+import launch
+import unittest
+import launch_ros
+import launch_testing
+from ament_index_python.packages import get_package_share_directory
+from moveit_configs_utils import MoveItConfigsBuilder
+
+
+def generate_test_description():
+    moveit_config = (
+        MoveItConfigsBuilder("moveit_resources_panda")
+        .robot_description(
+            file_path="config/panda.urdf.xacro",
+        )
+        .robot_description_semantic(file_path="config/panda.srdf")
+        .planning_scene_monitor(
+            publish_robot_description=True, publish_robot_description_semantic=True
+        )
+        .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")
+        .planning_pipelines(
+            pipelines=["ompl", "chomp", "pilz_industrial_motion_planner", "stomp"]
+        )
+        .to_moveit_configs()
+    )
+
+    # Start the actual move_group node/action server
+    move_group_node = launch_ros.actions.Node(
+        package="moveit_ros_move_group",
+        executable="move_group",
+        output="screen",
+        parameters=[moveit_config.to_dict()],
+        arguments=["--ros-args", "--log-level", "info"],
+    )
+
+    # ros2_control using FakeSystem as hardware
+    ros2_controllers_path = os.path.join(
+        get_package_share_directory("moveit_resources_panda_moveit_config"),
+        "config",
+        "ros2_controllers.yaml",
+    )
+    ros2_control_node = launch_ros.actions.Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[ros2_controllers_path],
+        remappings=[
+            ("/controller_manager/robot_description", "/robot_description"),
+        ],
+        output="screen",
+    )
+
+    joint_state_broadcaster_spawner = launch_ros.actions.Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=[
+            "joint_state_broadcaster",
+            "--controller-manager",
+            "/controller_manager",
+        ],
+        output="screen",
+    )
+
+    panda_arm_controller_spawner = launch_ros.actions.Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["panda_arm_controller", "-c", "/controller_manager"],
+    )
+
+    panda_hand_controller_spawner = launch_ros.actions.Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["panda_hand_controller", "-c", "/controller_manager"],
+    )
+
+    # Static TF
+    static_tf_node = launch_ros.actions.Node(
+        package="tf2_ros",
+        executable="static_transform_publisher",
+        name="static_transform_publisher",
+        output="log",
+        arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"],
+    )
+
+    # Publish TF
+    robot_state_publisher = launch_ros.actions.Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher",
+        output="both",
+        parameters=[moveit_config.robot_description],
+    )
+
+    move_group_gtest = launch_ros.actions.Node(
+        executable=launch.substitutions.PathJoinSubstitution(
+            [
+                launch.substitutions.LaunchConfiguration("test_binary_dir"),
+                "move_group_api_test",
+            ]
+        ),
+        parameters=[moveit_config.to_dict()],
+        output="screen",
+    )
+
+    return launch.LaunchDescription(
+        [
+            launch.actions.DeclareLaunchArgument(
+                name="test_binary_dir",
+                description="Binary directory of package "
+                "containing test executables",
+            ),
+            static_tf_node,
+            robot_state_publisher,
+            move_group_node,
+            ros2_control_node,
+            joint_state_broadcaster_spawner,
+            panda_arm_controller_spawner,
+            panda_hand_controller_spawner,
+            move_group_gtest,
+            # launch.actions.TimerAction(period=15.0, actions=[move_group_gtest]),
+            launch_testing.actions.ReadyToTest(),
+        ]
+    ), {
+        "move_group_gtest": move_group_gtest,
+    }
+
+
+class TestGTestWaitForCompletion(unittest.TestCase):
+    # Waits for test to complete, then waits a bit to make sure result files are generated
+    def test_gtest_run_complete(self, move_group_gtest):
+        self.proc_info.assertWaitForShutdown(move_group_gtest, timeout=4000.0)
+
+
+@launch_testing.post_shutdown_test()
+class TestGTestProcessPostShutdown(unittest.TestCase):
+    # Checks if the test has been completed with acceptable exit codes (successful codes)
+    def test_gtest_pass(self, proc_info, move_group_gtest):
+        launch_testing.asserts.assertExitCodes(proc_info, process=move_group_gtest)