Re-Enable Servo Tests (#603)

* Delete leftover of already ported servo_cpp_interface tests
moveit · Aug 16, 2021 · 691e3ee · 691e3ee
1 parent e0699e4
commit 691e3ee
Show file tree

Hide file tree

Showing 4 changed files with 44 additions and 343 deletions.
diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt
@@ -184,71 +184,58 @@ if(BUILD_TESTING)
   ament_add_gtest(test_low_pass_filter test/test_low_pass_filter.cpp)
   target_link_libraries(test_low_pass_filter ${SERVO_LIB_NAME})
 
-  # TODO(andyz): re-enable integration tests when they are less flakey.
-  # The issue is that the test completes successfully but a results file is not generated.
-
-  # # Unit test for ServoCalcs
+  # Unit test for ServoCalcs
+  # TODO (vatanaksoytezer): Rewrite this similar to enforce limits test
   # ament_add_gtest_executable(
   #   unit_test_servo_calcs
   #   test/unit_test_servo_calcs.cpp
   # )
   # target_link_libraries(unit_test_servo_calcs ${SERVO_LIB_NAME})
   # ament_target_dependencies(unit_test_servo_calcs ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost)
-  # add_ros_test(test/launch/unit_test_servo_calcs.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
+  # add_ros_test(test/launch/unit_test_servo_calcs.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
 
-  # # Servo integration launch test
-  # ament_add_gtest_executable(test_servo_integration
-  #     test/test_servo_interface.cpp
-  #     test/servo_launch_test_common.hpp
-  # )
-  # target_link_libraries(test_servo_integration ${SERVO_PARAM_LIB_NAME})
-  # ament_target_dependencies(test_servo_integration ${THIS_PACKAGE_INCLUDE_DEPENDS})
-  # add_ros_test(test/launch/test_servo_integration.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
-
-  # # Servo collision checking integration test
-  # ament_add_gtest_executable(test_servo_collision
-  #       test/test_servo_collision.cpp
-  #       test/servo_launch_test_common.hpp
-  # )
-  # target_link_libraries(test_servo_collision ${SERVO_PARAM_LIB_NAME})
-  # ament_target_dependencies(test_servo_collision ${THIS_PACKAGE_INCLUDE_DEPENDS})
-  # add_ros_test(test/launch/test_servo_collision.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
-
-  # # Servo singularity checking integration test
-  # ament_add_gtest_executable(test_servo_singularity
-  #       test/test_servo_singularity.cpp
-  #       test/servo_launch_test_common.hpp
-  # )
-  # target_link_libraries(test_servo_singularity ${SERVO_PARAM_LIB_NAME})
-  # ament_target_dependencies(test_servo_singularity ${THIS_PACKAGE_INCLUDE_DEPENDS})
-  # add_ros_test(test/launch/test_servo_singularity.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
-
-  # TODO(henningkayser): Port tests to ROS2
-  # servo_cpp_interface
-  # add_rostest_gtest(servo_cpp_interface_test
-  #   test/servo_cpp_interface_test.test
-  #   test/servo_cpp_interface_test.cpp
-  # )
-  # target_link_libraries(servo_cpp_interface_test
-  #   ${SERVO_LIB_NAME}
-  #   ${catkin_LIBRARIES}
-  # )
+  # Servo integration launch test
+  ament_add_gtest_executable(test_servo_integration
+      test/test_servo_interface.cpp
+      test/servo_launch_test_common.hpp
+  )
+  target_link_libraries(test_servo_integration ${SERVO_PARAM_LIB_NAME})
+  ament_target_dependencies(test_servo_integration ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  add_ros_test(test/launch/test_servo_integration.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
+
+  # Servo collision checking integration test
+  ament_add_gtest_executable(test_servo_collision
+        test/test_servo_collision.cpp
+        test/servo_launch_test_common.hpp
+  )
+  target_link_libraries(test_servo_collision ${SERVO_PARAM_LIB_NAME})
+  ament_target_dependencies(test_servo_collision ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  add_ros_test(test/launch/test_servo_collision.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
+
+  # Servo singularity checking integration test
+  ament_add_gtest_executable(test_servo_singularity
+        test/test_servo_singularity.cpp
+        test/servo_launch_test_common.hpp
+  )
+  target_link_libraries(test_servo_singularity ${SERVO_PARAM_LIB_NAME})
+  ament_target_dependencies(test_servo_singularity ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  add_ros_test(test/launch/test_servo_singularity.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
 
-  # # 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}")
+  # 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 TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
 
-  # # basic_servo_tests
-  # ament_add_gtest_executable(basic_servo_tests
-  #       test/basic_servo_tests.cpp
-  # )
-  # ament_target_dependencies(basic_servo_tests ${THIS_PACKAGE_INCLUDE_DEPENDS})
-  # target_link_libraries(basic_servo_tests ${POSE_TRACKING})
-  # add_ros_test(test/launch/test_basic_servo.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
+  # basic_servo_tests
+  ament_add_gtest_executable(basic_servo_tests
+        test/basic_servo_tests.cpp
+  )
+  ament_target_dependencies(basic_servo_tests ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  target_link_libraries(basic_servo_tests ${POSE_TRACKING})
+  add_ros_test(test/launch/test_basic_servo.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
 
   # Enforce limits unit tests
   ament_add_gtest(enforce_limits_tests

diff --git a/moveit_ros/moveit_servo/package.xml b/moveit_ros/moveit_servo/package.xml
@@ -36,6 +36,7 @@
   <exec_depend>joy</exec_depend>
   <exec_depend>tf2_ros</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>joint_state_controller</exec_depend>
 
   <test_depend>moveit_resources_panda_moveit_config</test_depend>
   <test_depend>ros_testing</test_depend>

diff --git a/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp b/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp