From 3e9b4dd661ec58f1239dce64e12fecc4aa56afc0 Mon Sep 17 00:00:00 2001 From: sebastian zarnack Date: Fri, 18 Jul 2025 11:28:17 +0200 Subject: [PATCH 1/4] 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 --- moveit_ros/moveit_servo/src/servo_node.cpp | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index 7b842c8ca3..848f3ad135 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/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 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)); From d6695a2b9bcea2f59524613dd73ea98b7ed81345 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Thu, 24 Jul 2025 14:30:26 -0500 Subject: [PATCH 2/4] Function naming --- moveit_ros/moveit_servo/src/servo_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index 848f3ad135..c16625e613 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -50,7 +50,7 @@ namespace moveit_servo { -rclcpp::Time convert_clock_type(const rclcpp::Time& time, rcl_clock_type_t new_clock_type) +rclcpp::Time convertClockType(const rclcpp::Time& time, rcl_clock_type_t new_clock_type) { if (time.get_clock_type() != new_clock_type) { @@ -337,7 +337,7 @@ void ServoNode::servoLoop() const auto servo_node_start = node_->now(); while (servo_node_start > - convert_clock_type(planning_scene_monitor_->getLastUpdateTime(), servo_node_start.get_clock_type())) + 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)); From 98ab363c46081794b0b9d7562ec476bfd366ce6a Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sat, 9 Aug 2025 10:44:23 -0500 Subject: [PATCH 3/4] Anonymous namespace --- moveit_ros/moveit_servo/src/servo_node.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index c16625e613..36a80fa40d 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -50,6 +50,9 @@ 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) @@ -58,6 +61,7 @@ rclcpp::Time convertClockType(const rclcpp::Time& time, rcl_clock_type_t new_clo } return time; } +} // namespace rclcpp::node_interfaces::NodeBaseInterface::SharedPtr ServoNode::get_node_base_interface() { From faa0d99fe73085844e1c25f691ec5b487a3efa9d Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 11 Aug 2025 11:47:23 -0500 Subject: [PATCH 4/4] Better comments --- moveit_ros/moveit_servo/src/servo_node.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index 36a80fa40d..ecaa486bec 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -340,6 +340,8 @@ void ServoNode::servoLoop() const auto servo_node_start = node_->now(); + // 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())) {