moveit_servo does not work until the joint position changes

[maxwelllls]

Description

A moveit_servo node was added to the robot package, but after starting it, the message "Waiting to receive robot state update." is continuously displayed, and the jog function cannot be executed. It only starts working normally after the robot is moved through other means. Upon investigation, it was found that in current_state_monitor.cpp line 357:

if (robot_state_.getJointPositions(jm)[0] != joint_state->position[i])

This logic requires that the robot’s joint position must change in order to send a new joint state.

Suggestion: Consider adding a timeout mechanism to update the joint state.

ROS Distro

Humble

OS and version

ubuntu 22.04

Source or binary build?

Source

If binary, which release version?

No response

If source, which branch?

main

Which RMW are you using?

CycloneDDS

Steps to Reproduce

A moveit_servo node was added to the robot package, hardware is mock joints. start package with

ros2 launch test_robot demo.launch.py

Expected behavior

moveit_servo should start and work correctly without requiring the robot's joint position to change. It should be able to receive and process jog commands immediately upon startup.

Actual behavior

Upon starting moveit_servo, the console displays the message "Waiting to receive robot state update," and the jog functionality does not work. The jog control only becomes functional after the robot is moved via other means, which triggers a joint state update.

Backtrace or Console output

[servo_node-2] [INFO] [1729777416.775937835] [slave_servo_node]: Waiting to receive robot state update. [servo_node-3] [INFO] [1729777417.759779353] [master_servo_node]: Waiting to receive robot state update. [servo_node-2] [INFO] [1729777417.776050432] [slave_servo_node]: Waiting to receive robot state update. [servo_node-3] [INFO] [1729777418.759894841] [master_servo_node]: Waiting to receive robot state update. [servo_node-2] [INFO] [1729777418.776130021] [slave_servo_node]: Waiting to receive robot state update.

[maxwelllls]

I tried adding a 1Hz forced update to the current_state_monitor, but updating the robot_state simultaneously triggers the update of /monitored_planning_scene. This causes the clock type of planning_scene_monitor_to be overwritten back to RCL_SYSTEM_TIME IMMEDIATELY.

For now, I had to temporarily disable the "/monitored_planning_scene"subscription in servo_node.

I am unsure why the last_update_time_clock type of planning_scene_monitorwas intentionally designed this way. It's quite confusing, and I would appreciate if someone could provide an explanation.

[sea-bass]

Curious about your use case?

I think this was added intentionally so a user wouldn't start jogging the arm from an uninitialized/out of sync state.

Where is your initial state coming from, if not this topic?

[maxwelllls]

I am testing the upper-level software developed for my robot, using a mock component for ros2_control hardware. This mock component returns a state value that always equals the command value, so the state remains constant at both startup and during static periods.

After adding forced update logic to current_state_monitor, I managed to bypass the issue. However, as previously mentioned, I have to disable the/monitored_planning_scene subscription. Otherwise, the clock type is overwritten to RCL_SYSTEM_TIME.