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 convertClockType() helper function to handle clock type conversions

Author: sebastian zarnack

moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp

@@ -103,8 +103,16 @@ class ServoNode
   std::optional<KinematicState> processTwistCommand(const moveit::core::RobotStatePtr& robot_state);
   std::optional<KinematicState> processPoseCommand(const moveit::core::RobotStatePtr& robot_state);
 
-  // Variables
+  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;
+  }
 
+  // Variables
   const rclcpp::Node::SharedPtr node_;
   std::unique_ptr<Servo> servo_;
   servo::Params servo_params_;

moveit_ros/moveit_servo/src/servo_node.cpp

@@ -325,10 +325,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 >
+         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));