diff --git a/moveit_ros/tests/CMakeLists.txt b/moveit_ros/tests/CMakeLists.txt index 43a87e9a99b..4f9a83b7f34 100644 --- a/moveit_ros/tests/CMakeLists.txt +++ b/moveit_ros/tests/CMakeLists.txt @@ -2,35 +2,18 @@ cmake_minimum_required(VERSION 3.22) project(moveit_ros_tests LANGUAGES CXX) # Common cmake code applied to all moveit packages -find_package(ament_cmake_python REQUIRED) find_package(moveit_common REQUIRED) moveit_package() if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) - find_package(ament_cmake_ros REQUIRED) - find_package(moveit_ros_planning REQUIRED) + find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(ros_testing REQUIRED) include_directories(include) ament_add_gtest_executable(move_group_api_test src/move_group_api_test.cpp) ament_target_dependencies(move_group_api_test rclcpp) - add_ros_test(launch/move_group_api.py TIMEOUT 30 ENV "ROS_DOMAIN_ID=2") - - ament_add_gtest_executable(move_group_default_planning_scene_test - src/move_group_default_planning_scene_test.cpp) - ament_target_dependencies(move_group_default_planning_scene_test rclcpp - moveit_ros_planning) - # run default_planning_scene test isolated test to avoid interference - add_ros_test(launch/move_group_default_planning_scene.py TIMEOUT 30 ENV - "ROS_DOMAIN_ID=2") - - # install Python helper script and test - ament_python_install_package(moveit_ros_tests) - install(DIRECTORY data DESTINATION share/${PROJECT_NAME}) - install(TARGETS move_group_api_test move_group_default_planning_scene_test - RUNTIME DESTINATION lib/${PROJECT_NAME}) + add_ros_test(launch/move_group_api.test.py TIMEOUT 30 ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") endif() - -ament_package() diff --git a/moveit_ros/tests/data/scene.txt b/moveit_ros/tests/data/scene.txt deleted file mode 100644 index 13ae3e59b06..00000000000 --- a/moveit_ros/tests/data/scene.txt +++ /dev/null @@ -1,22 +0,0 @@ -(noname)+ -* Box_0 -0 0.8 0.5 -0 0 0 1 -1 -box -0.2 0.2 0.2 -0 0 0 -0 0 0 1 -0 0 0 0 -0 -* Cylinder_0 -0.69 0 0.73 -0 0 0 1 -1 -cylinder -0.1 0.2 -0 0 0 -0 0 0 1 -0 0 0 0 -0 -. diff --git a/moveit_ros/tests/launch/move_group_api.py b/moveit_ros/tests/launch/move_group_api.py deleted file mode 100644 index 0dbdec64d6a..00000000000 --- a/moveit_ros/tests/launch/move_group_api.py +++ /dev/null @@ -1,56 +0,0 @@ -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 -from moveit_ros_tests.move_group_test_description import ( - generate_move_group_test_description, -) - - -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() - ) - - launch.actions.DeclareLaunchArgument( - name="test_binary_dir", - description="Binary directory of package " "containing test executables", - ) - move_group_gtest = launch_ros.actions.Node( - package="moveit_ros_tests", - executable="move_group_api_test", - parameters=[moveit_config.to_dict()], - output="screen", - ) - - return generate_move_group_test_description( - moveit_config.to_dict(), 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) diff --git a/moveit_ros/tests/launch/move_group_default_planning_scene.py b/moveit_ros/tests/launch/move_group_default_planning_scene.py deleted file mode 100644 index 05512838047..00000000000 --- a/moveit_ros/tests/launch/move_group_default_planning_scene.py +++ /dev/null @@ -1,58 +0,0 @@ -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 -from moveit_ros_tests.move_group_test_description import ( - generate_move_group_test_description, -) -import launch_testing -import pytest - - -@pytest.mark.launch_test -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() - ) - moveit_config_dict = moveit_config.to_dict() - moveit_config_dict["default_planning_scene"] = os.path.join( - get_package_share_directory("moveit_ros_tests"), "data", "scene.txt" - ) - - move_group_gtest = launch_ros.actions.Node( - package="moveit_ros_tests", - executable="move_group_default_planning_scene_test", - parameters=[moveit_config_dict], - output="screen", - ) - - return generate_move_group_test_description(moveit_config_dict, 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) diff --git a/moveit_ros/tests/moveit_ros_tests/__init__.py b/moveit_ros/tests/moveit_ros_tests/__init__.py deleted file mode 100644 index e69de29bb2d..00000000000 diff --git a/moveit_ros/tests/moveit_ros_tests/move_group_test_description.py b/moveit_ros/tests/moveit_ros_tests/move_group_test_description.py deleted file mode 100644 index fc1b455f24b..00000000000 --- a/moveit_ros/tests/moveit_ros_tests/move_group_test_description.py +++ /dev/null @@ -1,92 +0,0 @@ -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_move_group_test_description(moveit_config_dict, move_group_gtest): - # 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_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=[{"robot_description": moveit_config_dict["robot_description"]}], - ) - - return launch.LaunchDescription( - [ - 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, - } diff --git a/moveit_ros/tests/src/move_group_default_planning_scene_test.cpp b/moveit_ros/tests/src/move_group_default_planning_scene_test.cpp deleted file mode 100644 index f93bf7c2d93..00000000000 --- a/moveit_ros/tests/src/move_group_default_planning_scene_test.cpp +++ /dev/null @@ -1,74 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2024, PickNik Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of PickNik Inc. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Paul Gesel - Description: Integration tests for the move_group default planning scene behavior -*/ - -#include -#include -#include - -TEST(MoveGroup, testDefaultPlanningScene) -{ - // PlanningSceneMonitor is used for getting the robot joint state and calculating the Cartesian pose for a given link. - auto node = std::make_shared("move_group_test"); - auto planning_scene_monitor = - std::make_shared(node, "robot_description"); - ASSERT_TRUE(planning_scene_monitor->requestPlanningSceneState()) << "Failed to get planning scene"; - rclcpp::sleep_for(std::chrono::seconds(5)); - ASSERT_TRUE(planning_scene_monitor->requestPlanningSceneState()) << "Failed to get planning scene"; - planning_scene::PlanningScenePtr ps = planning_scene_monitor->getPlanningScene(); - - // ensure the planning scene and world are not null - ASSERT_NE(ps, nullptr) << "PlanningScene was null"; - ASSERT_NE(ps->getWorld(), nullptr) << "PlanningScene->getWorld() was null"; - - // check that geometry in test file matches the currecnt values - std::unordered_set expected_ids = { "Box_0", "Cylinder_0" }; - EXPECT_EQ(ps->getWorld()->getObjectIds().size(), 2ul); - for (const auto& id : ps->getWorld()->getObjectIds()) - { - EXPECT_NE(expected_ids.find(id), expected_ids.end()) << "Unexpected object id"; - } -} - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - ::testing::InitGoogleTest(&argc, argv); - int result = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return result; -}