fixed moveit_servo to work with fake_hardware (fixes issue #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

Author: sebastian zarnack

moveit_ros/moveit_servo/src/servo_node.cpp

@@ -50,6 +50,15 @@
 namespace moveit_servo
 {
 
+rclcpp::Time convert_clock_type(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;
+}
+
 rclcpp::node_interfaces::NodeBaseInterface::SharedPtr ServoNode::get_node_base_interface()
 {
   return node_->get_node_base_interface();
@@ -325,10 +334,14 @@ 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())
+
+  while (servo_node_start >
+         convert_clock_type(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));