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.