diff --git a/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp b/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp index e9bc3414ea..9d813402b5 100644 --- a/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp +++ b/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp @@ -66,9 +66,6 @@ TEST(RDFIntegration, topic_based) EXPECT_EQ("gonzo", loader.getSRDF()->getName()); } -// Failing in Jammy disabling temporarily (TODO (vatanaksoytezer): Fix this and enable again) -// See https://github.com/ros-planning/moveit2/issues/1156 -/* TEST(RDFIntegration, executor) { // RDFLoader should successfully load URDF and SRDF strings from a ROS topic when the node that is @@ -77,11 +74,9 @@ TEST(RDFIntegration, executor) rclcpp::Node::SharedPtr node = std::make_shared("executor"); // Create a thread to spin an Executor. - std::thread([node]() { - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node); - executor.spin(); - }).detach(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + std::thread spinning_thread([&executor] { executor.spin(); }); // WHEN the RDFLoader is created rdf_loader::RDFLoader loader(node, "topic_description"); @@ -91,8 +86,9 @@ TEST(RDFIntegration, executor) EXPECT_EQ("gonzo", loader.getURDF()->name_); ASSERT_NE(nullptr, loader.getSRDF()); EXPECT_EQ("gonzo", loader.getSRDF()->getName()); + executor.cancel(); + spinning_thread.join(); } -*/ TEST(RDFIntegration, xacro_test) {