ServoCppFixture.PoseTest is flaky

[rhaschke]

Description

failure: https://github.com/moveit/moveit2/actions/runs/10915496106/job/30295360965
success: https://github.com/moveit/moveit2/actions/runs/10915496106

ROS Distro

Rolling

OS and version

Ubuntu 24.04

Source or binary build?

Source

If binary, which release version?

No response

If source, which branch?

main

Which RMW are you using?

None

Steps to Reproduce

run CI actions

Expected behavior

tests should always succeed

Actual behavior

sometimes the test fails

Backtrace or Console output

No response

[sea-bass]

The "bad" status code for this test indicates that the robot is halting for collision.

I guess picking a safer target pose could be helpful, but I think using a full IK solver instead of something simpler/more conducive to online control like a Jacobian pseudoinverse could also be a cause.

[sea-bass]

Attempting something in #3007 -- but unclear if this will help.

[sea-bass]

I think some final tweaks in #3007 may have helped with flakiness. If anyone sees the servo tests still failing, please post on this issue. Would be good to monitor it for a while before calling this "solved".

[rhaschke]

I just saw another test failure: https://github.com/moveit/moveit2/actions/runs/11203600643/job/31141038313?pr=3020

[mikeferguson]

Copying the text here - since the github CI runs eventually disappear:

    ======================================================================
    FAIL: test_gtest_pass (moveit_servo.TestGTestProcessPostShutdown.test_gtest_pass)
    ----------------------------------------------------------------------
    Traceback (most recent call last):
      File "/home/runner/work/moveit2/moveit2/.work/target_ws/src/moveit2/moveit_ros/moveit_servo/tests/launch/servo_cpp_integration.test.py", line 133, in test_gtest_pass
        launch_testing.asserts.assertExitCodes(proc_info, process=servo_gtest)
      File "/opt/ros/jazzy/lib/python3.12/site-packages/launch_testing/asserts/assert_exit_codes.py", line 62, in assertExitCodes
        assert info.returncode in allowable_exit_codes, 'Proc {} exited with code {}'.format(
               ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
    AssertionError: Proc moveit_servo_cpp_integration_test-5 exited with code 1
    
    ----------------------------------------------------------------------
    Ran 1 test in 0.001s
    
    FAILED (failures=1)
    -- run_test.py: return code 1
    -- run_test.py: verify result file '/home/runner/work/moveit2/moveit2/.work/target_ws/build/moveit_servo/test_results/moveit_servo/tests_launch_servo_cpp_integration.test.py.xunit.xml'
  >>>
build/moveit_servo/test_results/moveit_servo/tests_launch_servo_cpp_integration.test.py.xunit.xml: 2 tests, 0 errors, 1 failure, 0 skipped
- moveit_servo.TestGTestProcessPostShutdown test_gtest_pass
  <<< failure message
    Traceback (most recent call last):
      File "/home/runner/work/moveit2/moveit2/.work/target_ws/src/moveit2/moveit_ros/moveit_servo/tests/launch/servo_cpp_integration.test.py", line 133, in test_gtest_pass
        launch_testing.asserts.assertExitCodes(proc_info, process=servo_gtest)
      File "/opt/ros/jazzy/lib/python3.12/site-packages/launch_testing/asserts/assert_exit_codes.py", line 62, in assertExitCodes
        assert info.returncode in allowable_exit_codes, 'Proc {} exited with code {}'.format(
               ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
    AssertionError: Proc moveit_servo_cpp_integration_test-5 exited with code 1
  >>>

[sea-bass]

Thanks @mikeferguson -- the actual error is a bit farther up, and I still have no idea what causes it.

/home/runner/work/moveit2/moveit2/.work/target_ws/src/moveit2/moveit_ros/moveit_servo/tests/test_integration.cpp:141: Failure
    [moveit_servo_cpp_integration_test-5] Expected equality of these values:
    [moveit_servo_cpp_integration_test-5]   status_curr
    [moveit_servo_cpp_integration_test-5]     Which is: 1-byte object <05>
    [moveit_servo_cpp_integration_test-5]   moveit_servo::StatusCode::NO_WARNING
    [moveit_servo_cpp_integration_test-5]     Which is: 1-byte object <00>
    [moveit_servo_cpp_integration_test-5]
[moveit_servo_cpp_integration_test-5] [  FAILED  ] ServoCppFixture.PoseTest (170 ms)

According to the enums in https://github.com/moveit/moveit_msgs/blob/ros2/msg/ServoStatus.msg, it apparently is moving the arm into collision.

It could be that the whole bringup is sporadically broken, so the robot sometimes starts at zero configuration (which is in collision). I sometimes see this happening when I run some of the MoveIt demos locally, especially when not using CycloneDDS.