From 27d9d56ec4698e0787827fae9196880f996e953f Mon Sep 17 00:00:00 2001 From: sea-bass Date: Thu, 21 Nov 2024 21:14:28 -0500 Subject: [PATCH] Re-enable flaky PSM test and increase timeout --- .../planning/planning_scene_monitor/CMakeLists.txt | 2 ++ .../test/launch/planning_scene_monitor.test.py | 9 ++++++--- .../test/planning_scene_monitor_test.cpp | 3 +++ 3 files changed, 11 insertions(+), 3 deletions(-) diff --git a/moveit_ros/planning/planning_scene_monitor/CMakeLists.txt b/moveit_ros/planning/planning_scene_monitor/CMakeLists.txt index 8aa26aa4a7..03eab4077d 100644 --- a/moveit_ros/planning/planning_scene_monitor/CMakeLists.txt +++ b/moveit_ros/planning/planning_scene_monitor/CMakeLists.txt @@ -49,6 +49,7 @@ if(BUILD_TESTING) test/current_state_monitor_tests.cpp) target_link_libraries(current_state_monitor_tests moveit_planning_scene_monitor) + ament_add_gmock(trajectory_monitor_tests test/trajectory_monitor_tests.cpp) target_link_libraries(trajectory_monitor_tests moveit_planning_scene_monitor) @@ -58,6 +59,7 @@ if(BUILD_TESTING) moveit_planning_scene_monitor) ament_target_dependencies(planning_scene_monitor_test moveit_core rclcpp moveit_msgs) + add_ros_test(test/launch/planning_scene_monitor.test.py TIMEOUT 30 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") endif() diff --git a/moveit_ros/planning/planning_scene_monitor/test/launch/planning_scene_monitor.test.py b/moveit_ros/planning/planning_scene_monitor/test/launch/planning_scene_monitor.test.py index 0f785d4d7d..98de74d786 100644 --- a/moveit_ros/planning/planning_scene_monitor/test/launch/planning_scene_monitor.test.py +++ b/moveit_ros/planning/planning_scene_monitor/test/launch/planning_scene_monitor.test.py @@ -46,7 +46,12 @@ def generate_test_description(): panda_arm_controller_spawner = launch_ros.actions.Node( package="controller_manager", executable="spawner", - arguments=["panda_arm_controller", "-c", "/controller_manager"], + arguments=[ + "panda_arm_controller", + "--controller-manager", + "/controller_manager", + ], + output="screen", ) psm_gtest = launch_ros.actions.Node( @@ -80,7 +85,6 @@ def generate_test_description(): class TestGTestWaitForCompletion(unittest.TestCase): - @unittest.skip("Flaky test on humble, see moveit2#2821") # Waits for test to complete, then waits a bit to make sure result files are generated def test_gtest_run_complete(self, psm_gtest): self.proc_info.assertWaitForShutdown(psm_gtest, timeout=4000.0) @@ -88,7 +92,6 @@ def test_gtest_run_complete(self, psm_gtest): @launch_testing.post_shutdown_test() class TestGTestProcessPostShutdown(unittest.TestCase): - @unittest.skip("Flaky test on humble, see moveit2#2821") # Checks if the test has been completed with acceptable exit codes (successful codes) def test_gtest_pass(self, proc_info, psm_gtest): launch_testing.asserts.assertExitCodes(proc_info, process=psm_gtest) diff --git a/moveit_ros/planning/planning_scene_monitor/test/planning_scene_monitor_test.cpp b/moveit_ros/planning/planning_scene_monitor/test/planning_scene_monitor_test.cpp index 806a88f3ac..0547216437 100644 --- a/moveit_ros/planning/planning_scene_monitor/test/planning_scene_monitor_test.cpp +++ b/moveit_ros/planning/planning_scene_monitor/test/planning_scene_monitor_test.cpp @@ -59,6 +59,9 @@ class PlanningSceneMonitorTest : public ::testing::Test scene_ = planning_scene_monitor_->getPlanningScene(); executor_->add_node(test_node_); executor_thread_ = std::thread([this]() { executor_->spin(); }); + + // Needed to avoid race conditions on high-load CPUs. + std::this_thread::sleep_for(std::chrono::seconds{ 1 }); } void TearDown() override