Use explicit executor in CPP integration test as well

moveit · Aug 10, 2024 · 4fd2200 · 4fd2200
1 parent 3b718c7
commit 4fd2200
Show file tree

Hide file tree

Showing 3 changed files with 24 additions and 7 deletions.
diff --git a/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp b/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp
@@ -64,6 +64,19 @@ class ServoCppFixture : public testing::Test
 
     servo_test_instance_ =
         std::make_shared<moveit_servo::Servo>(servo_test_node_, servo_param_listener_, planning_scene_monitor_);
+
+    executor_.add_node(servo_test_node_);
+    executor_thread_ = std::thread([this]() { executor_.spin(); });
+  }
+
+  void TearDown() override
+  {
+    executor_.remove_node(servo_test_node_);
+    executor_.cancel();
+    if (executor_thread_.joinable())
+    {
+      executor_thread_.join();
+    }
   }
 
   /// Helper function to get the current pose of a specified frame.
@@ -73,6 +86,11 @@ class ServoCppFixture : public testing::Test
   }
 
   std::shared_ptr<rclcpp::Node> servo_test_node_;
+
+  // Executor and a thread to run the executor.
+  rclcpp::executors::SingleThreadedExecutor executor_;
+  std::thread executor_thread_;
+
   std::shared_ptr<const servo::ParamListener> servo_param_listener_;
   servo::Params servo_params_;
   std::shared_ptr<moveit_servo::Servo> servo_test_instance_;

diff --git a/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp b/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp
@@ -93,10 +93,9 @@ class ServoRosFixture : public testing::Test
     switch_input_client_ =
         servo_test_node_->create_client<moveit_msgs::srv::ServoCommandType>("/servo_node/switch_command_type");
 
-    waitForService();
-
     executor_.add_node(servo_test_node_);
     executor_thread_ = std::thread([this]() { executor_.spin(); });
+    waitForService();
   }
 
   void TearDown() override
@@ -120,7 +119,7 @@ class ServoRosFixture : public testing::Test
         std::exit(EXIT_FAILURE);
       }
 
-      rclcpp::sleep_for(std::chrono::milliseconds(500));
+      std::this_thread::sleep_for(std::chrono::milliseconds(500));
     }
     RCLCPP_INFO(logger, "MoveIt Servo input switching service ready!");
   }
@@ -133,7 +132,7 @@ class ServoRosFixture : public testing::Test
 
     while (rclcpp::ok() && state_count_ == 0)
     {
-      rclcpp::sleep_for(std::chrono::milliseconds(500));
+      std::this_thread::sleep_for(std::chrono::milliseconds(500));
       auto elapsed_time = servo_test_node_->now() - start_time;
       if (elapsed_time >= wait_timeout)
       {

diff --git a/moveit_ros/moveit_servo/tests/test_ros_integration.cpp b/moveit_ros/moveit_servo/tests/test_ros_integration.cpp
@@ -98,7 +98,7 @@ TEST_F(ServoRosFixture, testJointJog)
     jog_cmd.header.stamp = servo_test_node_->now();
     joint_jog_publisher->publish(jog_cmd);
     count++;
-    rclcpp::sleep_for(std::chrono::milliseconds(200));
+    std::this_thread::sleep_for(std::chrono::milliseconds(200));
   }
 
   ASSERT_GE(traj_count_, NUM_COMMANDS);
@@ -137,7 +137,7 @@ TEST_F(ServoRosFixture, testTwist)
     twist_cmd.header.stamp = servo_test_node_->now();
     twist_publisher->publish(twist_cmd);
     count++;
-    rclcpp::sleep_for(std::chrono::milliseconds(200));
+    std::this_thread::sleep_for(std::chrono::milliseconds(200));
   }
 
   ASSERT_GE(traj_count_, NUM_COMMANDS);
@@ -178,7 +178,7 @@ TEST_F(ServoRosFixture, testPose)
     pose_cmd.header.stamp = servo_test_node_->now();
     pose_publisher->publish(pose_cmd);
     count++;
-    rclcpp::sleep_for(std::chrono::milliseconds(200));
+    std::this_thread::sleep_for(std::chrono::milliseconds(200));
   }
 
   ASSERT_GE(traj_count_, NUM_COMMANDS);