Adjust moveit_servo clock to work with fake_hardware

[Zarnack]

This PR resolves an initialization issue in moveit_servo when used with ros2_control mock hardware components. The robot must be manually moved at startup to trigger joint state updates, otherwise moveit_servo fails to initialize due to a mismatch in clock types between the PlanningSceneMonitor and the ServoNode.

This fixes issue #3040

Description

At startup, servo_node checks whether a recent robot_state has been received by comparing the current time with the timestamp of the latest update (last_update_time_). However, when using a ros2_control mock component, it may wait indefinitely—even if joint states are being published regularly. This issue stems from a mismatch in clock types between the PlanningSceneMonitor and the moveit_servo node.

Mock components typically publish ideal, noise-free joint states, which results in the robot appearing idle.
Consequently:

• The joint state doesn't change.
• PlanningSceneMonitor does not update last_update_time_ using RCL_ROS_TIME. (it only updates when the joint states change)
  • Instead, last_update_time_ is updated with RCL_SYSTEM_TIME.
• moveit_servo, however, performs its time comparisons using RCL_ROS_TIME.
• This discrepancy causes the freshness check to fail, preventing initialization.

Solution:

This PR introduces a clock type conversion to ensure that time comparisons between moveit_servo and PlanningSceneMonitor are consistent, regardless of the underlying clock type.
This resolves the startup issue when using mock components that produce static, noise-free joint states and therefore do not trigger joint state updates.

There are no relevant API changes.

Checklist

• Required by CI: Code is auto formatted using clang-format
• Extend the tutorials / documentation reference
• Document API changes relevant to the user in the [MIGRATION.md] (https://github.com/moveit/moveit2/blob/main/MIGRATION.md) notes
• Create tests, which fail without this PR reference
• Include a screenshot if changing a GUI
• While waiting for someone to review your request, please help review another open pull request to support the maintainers

[codecov]

Codecov Report

✅ All modified and coverable lines are covered by tests.
✅ Project coverage is 46.19%. Comparing base (4cbeb9b) to head (faa0d99).
⚠️ Report is 1 commits behind head on main.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3529      +/-   ##
==========================================
- Coverage   46.21%   46.19%   -0.02%     
==========================================
  Files         720      720              
  Lines       62742    59180    -3562     
  Branches     7594     7595       +1     
==========================================
- Hits        28991    27330    -1661     
+ Misses      33584    31683    -1901     
  Partials      167      167              

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[AndyZe]

Thanks. I'm good to merge this if CI passes

[AndyZe]

Maybe @nbbrooks will push this across the finish line

[MarqRazz]

PlanningSceneMonitor does not update last_update_time_ using RCL_ROS_TIME. (it only updates when the joint states change)
Instead, last_update_time_ is updated with RCL_SYSTEM_TIME.

This sounds like the actual issue. Have we dug into why it is hard coded to SYSTEM_TIME?

[AndyZe]

This sounds like the actual issue. Have we dug into why it is hard coded to SYSTEM_TIME?

I don't think anybody has the appetite to make changes in planning_scene_monitor right now. I'll make the anonymous namespace change, though

[MarqRazz]

Add a note of why we needed to add the function and I think it's good.

[Zarnack]

@AndyZe @nbbrooks any update on this?

[AndyZe]

I would merge it if I were able to...

[nbbrooks]

Are the 3 failing checks something new? That is why it has not already been auto-merged.

I can bypass the checks but it'd be good to know what is going on there.

[nbbrooks]

The checks are also failing on this PR we have been wanting to merge in for a while #2994 and I have re-run them a couple times. So if something is flaky, it must be fairly reliably flaky at this point?

[nbbrooks]

Looks like they are related to TF2 h to hpp deprecations https://github.com/moveit/moveit2/actions/runs/16886460567/job/47835375146?pr=3529#step:11:1188

Which #3533 and #3562 have started to address.

[AndyZe]

I'm going to attempt to rebase and merge 👍