Adjust moveit_servo clock to work with fake_hardware (moveit#3529)

* fixed moveit_servo to work with fake_hardware (fixes issue moveit#3040)

The planning scene monitor may use a different clock type (e.g., system clock)
than the servo node (ROS clock), causing timestamp comparison failures that
prevented servo from starting properly with fake_hardware controllers.
--> Add convert_clock_type() helper function to handle clock type conversions

* Function naming

* Anonymous namespace

* Better comments

---------

Co-authored-by: sebastian zarnack <sebastian.zarnack@eas.iis.fraunhofer.de>
Co-authored-by: AndyZe <andyz@utexas.edu>

Author: 3 people

moveit_ros/moveit_servo/src/servo_node.cpp

@@ -50,6 +50,19 @@
 namespace moveit_servo
 {
 
+namespace
+{
+// This function is used to convert times in case planning_scene_monitor uses RCL_SYSTEM_TIME
+rclcpp::Time convertClockType(const rclcpp::Time& time, rcl_clock_type_t new_clock_type)
+{
+  if (time.get_clock_type() != new_clock_type)
+  {
+    return rclcpp::Time(time.nanoseconds(), new_clock_type);
+  }
+  return time;
+}
+}  // namespace
+
 rclcpp::node_interfaces::NodeBaseInterface::SharedPtr ServoNode::get_node_base_interface()
 {
   return node_->get_node_base_interface();
@@ -325,10 +338,16 @@ void ServoNode::servoLoop()
   std::optional<KinematicState> next_joint_state = std::nullopt;
   rclcpp::WallRate servo_frequency(1 / servo_params_.publish_period);
 
-  // wait for first robot joint state update
   const auto servo_node_start = node_->now();
-  while (planning_scene_monitor_->getLastUpdateTime().get_clock_type() != node_->get_clock()->get_clock_type() ||
-         servo_node_start > planning_scene_monitor_->getLastUpdateTime())
+
+  // convert_clock_type() is used in case planning_scene_monitor uses RCL_SYSTEM_TIME
+  // while Servo uses RCL_ROS_TIME
+  while (servo_node_start >
+         convertClockType(planning_scene_monitor_->getLastUpdateTime(), servo_node_start.get_clock_type()))
+  {
+    RCLCPP_INFO(node_->get_logger(), "Waiting for planning scene monitor to receive robot state update.");
+    rclcpp::sleep_for(std::chrono::seconds(1));
+  }
   {
     RCLCPP_INFO(node_->get_logger(), "Waiting to receive robot state update.");
     rclcpp::sleep_for(std::chrono::seconds(1));