From c134d89710e831ae58a66e53d87a78b44b0b307c Mon Sep 17 00:00:00 2001
From: AndyZe <zelenak@picknik.ai>
Date: Tue, 30 Aug 2022 07:51:00 -0500
Subject: [PATCH] [Servo] Use a WallRate so the clock is monotonically
 increasing (#1543)

* [Servo] Use a WallRate so the clock is monotonically increasing

* Re-enable a commented integration test
---
 moveit_ros/moveit_servo/CMakeLists.txt        | 15 +++++++--------
 moveit_ros/moveit_servo/src/pose_tracking.cpp |  3 +--
 moveit_ros/moveit_servo/src/servo_calcs.cpp   |  2 +-
 3 files changed, 9 insertions(+), 11 deletions(-)

diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt
index 762e631358..1ebdb8ee32 100644
--- 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
index fed500dfad..c715d4ebc3 100644
--- 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
index 9ddaac74ca..d4c23201ec 100644
--- 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_)
   {