Re-re-enable servo tests.

moveit_ros/moveit_servo/CMakeLists.txt

@@ -243,12 +243,12 @@ if(BUILD_TESTING)
 
   # Flaky, disabling until it is fixed
   # pose_tracking
-  # ament_add_gtest_executable(test_servo_pose_tracking
-  #       test/pose_tracking_test.cpp
-  # )
-  # ament_target_dependencies(test_servo_pose_tracking ${THIS_PACKAGE_INCLUDE_DEPENDS})
-  # target_link_libraries(test_servo_pose_tracking ${POSE_TRACKING})
-  # add_ros_test(test/launch/test_servo_pose_tracking.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
+  ament_add_gtest_executable(test_servo_pose_tracking
+        test/pose_tracking_test.cpp
+  )
+  ament_target_dependencies(test_servo_pose_tracking ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  target_link_libraries(test_servo_pose_tracking ${POSE_TRACKING})
+  add_ros_test(test/launch/test_servo_pose_tracking.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
 
   # # low_latency
   # add_rostest_gtest(servo_low_latency_test

moveit_ros/moveit_servo/test/pose_tracking_test.cpp

@@ -209,8 +209,8 @@ TEST_F(PoseTrackingFixture, OutgoingMsgTest)
 
 int main(int argc, char** argv)
 {
-  ::testing::InitGoogleTest(&argc, argv);
   rclcpp::init(argc, argv);
+  ::testing::InitGoogleTest(&argc, argv);
 
   int result = RUN_ALL_TESTS();
   rclcpp::shutdown();

moveit_ros/moveit_servo/test/test_servo_interface.cpp

@@ -222,8 +222,8 @@ TEST_F(ServoFixture, DynamicParameterTest)
 
 int main(int argc, char** argv)
 {
-  ::testing::InitGoogleTest(&argc, argv);
   rclcpp::init(argc, argv);
+  ::testing::InitGoogleTest(&argc, argv);
 
   int ret = RUN_ALL_TESTS();
   rclcpp::shutdown();

moveit_ros/moveit_servo/test/test_servo_singularity.cpp

@@ -100,8 +100,8 @@ TEST_F(ServoFixture, ReachSingular)
 
 int main(int argc, char** argv)
 {
-  ::testing::InitGoogleTest(&argc, argv);
   rclcpp::init(argc, argv);
+  ::testing::InitGoogleTest(&argc, argv);
 
   int ret = RUN_ALL_TESTS();
   rclcpp::shutdown();

moveit_ros/moveit_servo/test/unit_test_servo_calcs.cpp

@@ -363,7 +363,7 @@ TEST_F(ServoCalcsTestFixture, TestEnforceVelLimits)
     EXPECT_LE(desired_velocity[i], vel_limits[i] * servo_calcs_->parameters_->publish_period);
   }
 
-  // Let's check the negative velocities too
+  // Let's check negative velocity limits too
   desired_velocity *= -1;
   servo_calcs_->prev_joint_velocity_ = desired_velocity;
   servo_calcs_->enforceVelLimits(desired_velocity);
@@ -505,8 +505,8 @@ TEST_F(ServoCalcsTestFixture, TestComposeOutputMsg)
 
 int main(int argc, char** argv)
 {
-  ::testing::InitGoogleTest(&argc, argv);
   rclcpp::init(argc, argv);
+  ::testing::InitGoogleTest(&argc, argv);
 
   // Set up the shared node
   TEST_NODE = std::make_shared<rclcpp::Node>("servo_calcs_test");

moveit_ros/planning_interface/test/move_group_ompl_constraints_test.cpp

@@ -174,8 +174,8 @@ TEST_F(ConstrainedPlanningTestFixture, PositionConstraint)
 
 int main(int argc, char** argv)
 {
-  ::testing::InitGoogleTest(&argc, argv);
   rclcpp::init(argc, argv);
+  ::testing::InitGoogleTest(&argc, argv);
 
   int ret = RUN_ALL_TESTS();
   rclcpp::shutdown();