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_monitor was 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.

[github-actions[bot]]

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.

[Zarnack]

I have just stumbled across the same issue: When using moveit_servo with a mock component for ros2_control hardware, the robot arm must first be moved to update the joint state. Without this, moveit_servo fails to acknowledge the given planning scene due to a mismatch in clock types used in the PlanningSceneMonitor and the ServoNode.

Root Cause

The problem arises because the PlanningSceneMonitor uses two different clock types for the last_update_time_ variable:

1. When a new joint state is received (and is different from the last one): RCL_ROS_TIME

   last_update_time_ = last_robot_motion_time_ = current_state_monitor_->getCurrentStateTime(); // RCL_ROS_TIME
   

2. In all other cases (e.g. regular updates with no differences in joint state): RCL_SYSTEM_TIME

   last_update_time_ = rclcpp::Clock().now(); // RCL_SYSTEM_TIME
   

This causes problems in moveit_servo

At line 330/331 of servo_node.cpp, there is a check ensuring that the PlanningSceneMonitor uses the same clock type as the ServoNode:

while (planning_scene_monitor_->getLastUpdateTime().get_clock_type() != node_->get_clock()->get_clock_type() ||
       servo_node_start > planning_scene_monitor_->getLastUpdateTime())

Why this causes issues with mock components

• Mock components typically send ideal, non-noisy joint state values.
• If the robot is idle, there are no differences in joint states, so the current state monitor does not update them. --> Consequently, last_update_time_ remains set using RCL_SYSTEM_TIME, which does not match the Servo node’s RCL_ROS_TIME. --> On real hardware, this is not an issue because joint state readings are naturally noisy, triggering state updates that ensure both clocks align properly, even when the robot is idle.

Possible solutions:

1. Modify moveit_servo:
   • Change the check on lines 330/331 so that it no longer verifies whether the clock types are the same. Instead, it should simply check if servo_node_start is later than planning_scene_monitor_->getLastUpdateTime(). However, since you can only compare times with the same clock type, first convert the timestamp returned by getLastUpdateTime() to the same clock type as the servo node’s time before performing the comparison.
   • This can be done via a simple header function:

       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;
       }
     

     and then the check can be rewritten to:

     while (servo_node_start > convertClockType(planning_scene_monitor_->getLastUpdateTime(), servo_node_start.get_clock_type()))
     

2. Unify the use of clock types in the PlanningSceneMonitor:
   • Ensure that last_update_time_ is always set using the same clock type. (either RCL_ROS_TIME or RCL_SYSTEM_TIME).
   • I am not sure if this is a good idea, as it might break other parts of the code that rely on the current behavior.

For the first solution I could provide a PR.

[FarStryke21]

Encountered the same issue. Can one of the maintainers complete the PR if it fixes the issue?

[maxwelllls]

Encountered the same issue. Can one of the maintainers complete the PR if it fixes the issue?

After starting all the programs, a trajectory topic that makes movegroup move forward slightly was sent through a python script, temporarily avoiding this problem