[Servo] Use a WallRate so the clock is monotonically increasing (move…

…it#1543) * [Servo] Use a WallRate so the clock is monotonically increasing * Re-enable a commented integration test
sjahr · Aug 30, 2022 · c134d89 · c134d89
1 parent 43a22ec
commit c134d89
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 11 deletions.
diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt
@@ -209,15 +209,14 @@ if(BUILD_TESTING)
   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}")
 
-  # TODO(sjahr): Debug and re-enable this test
   # 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}")
+  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

diff --git a/moveit_ros/moveit_servo/src/pose_tracking.cpp b/moveit_ros/moveit_servo/src/pose_tracking.cpp
@@ -114,7 +114,6 @@ PoseTrackingStatusCode PoseTracking::moveToPose(const Eigen::Vector3d& positiona
   // Wait a bit for a target pose message to arrive.
   // The target pose may get updated by new messages as the robot moves (in a callback function).
   const rclcpp::Time start_time = node_->now();
-  rclcpp::Clock clock;
 
   while ((!haveRecentTargetPose(target_pose_timeout) || !haveRecentEndEffectorPose(target_pose_timeout)) &&
          ((node_->now() - start_time).seconds() < target_pose_timeout))
@@ -170,7 +169,7 @@ PoseTrackingStatusCode PoseTracking::moveToPose(const Eigen::Vector3d& positiona
 
     if (!loop_rate_.sleep())
     {
-      RCLCPP_WARN_STREAM_THROTTLE(LOGGER, clock, LOG_THROTTLE_PERIOD, "Target control rate was missed");
+      RCLCPP_WARN_STREAM_THROTTLE(LOGGER, *node_->get_clock(), LOG_THROTTLE_PERIOD, "Target control rate was missed");
     }
   }
 

diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp
@@ -327,7 +327,7 @@ void ServoCalcs::stop()
 
 void ServoCalcs::mainCalcLoop()
 {
-  rclcpp::Rate rate(1.0 / parameters_->publish_period);
+  rclcpp::WallRate rate(1.0 / parameters_->publish_period);
 
   while (rclcpp::ok() && !stop_requested_)
   {