gigantic speed up to move group interface

[azalutsky]

Description

This speedup removes the 100ms delay during a planning time, which increases planning times by a substantial amount.
For example, my 2 robot arms without this takes 600ms+ to plan. By reducing this sleep to 1ms the plans are then in the 150ms range.

Please explain the changes you made, including a reference to the related issue if applicable

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

Base: 50.43% // Head: 50.42% // Decreases project coverage by -0.01% ⚠️

Coverage data is based on head (bd8891e) compared to base (6711436).
Patch has no changes to coverable lines.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #1865      +/-   ##
==========================================
- Coverage   50.43%   50.42%   -0.00%     
==========================================
  Files         374      374              
  Lines       31335    31335              
==========================================
- Hits        15801    15799       -2     
- Misses      15534    15536       +2     

Impacted Files	Coverage Δ
...nning_scene_monitor/src/planning_scene_monitor.cpp	45.26% <0.00%> (-0.43%)	⬇️
moveit_core/robot_state/src/robot_state.cpp	47.87% <0.00%> (-0.07%)	⬇️
...dl_kinematics_plugin/src/kdl_kinematics_plugin.cpp	79.93% <0.00%> (+1.14%)	⬆️

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

☔ View full report at Codecov.
📢 Do you have feedback about the report comment? Let us know in this issue.

[azalutsky]

Thanks guys, if anyone has any other tricks to speed up planning times- I'd love to know!

[azalutsky]

I am noticing however with multiple robots that there is a bug somewhere with these actions.

Independent of the timeout, but the timeout alleviates it a little bit.

For dual robot, the done flag claims it plans both and completes it however only the first robot in the group executes the trajectory while the second robot stays put.

Seems to be a bug. I started tracking it but if someone has an idea I'd love to hear your thoughts as I investigate as I am super new with the codebase.

I posted in discord's bug's channel about it.

Would love a second opinion.

[rhaschke]

@ros-planning/moveit-maintainers, why does this code use busy waiting??? There are condition variables for this purpose...

[samialperen]

@azalutsky I did not see a performance increase even after this PR. I was expecting it would increase the performance but did not happen. Did you manage to find any other tricks to decrease planning time for humble?

[azalutsky]

If you use move group interface, when you call plan the action has this 100ms latency.
What planning times are you seeing?

[samialperen]

@azalutsky I am using Move group interface. I actually applied this fix too and it did not change anything. I am getting around 600ms planning time which does not make sense to me at all. For the same setup, I am getting less than 100ms in moveit ROS1

[azalutsky]

Is it possible you're not using moveit2 via source? Did you perhaps install moveit2 through apt-get or didn't colcon build with making sure that the package was built by source? I believe foxy requires some additional output. 600ms is almost guarantee that this change didn't go through from your source -> binary.

[samialperen]

I built it from source and ros2 pkg prefix shows my workspace. For the reference, I am using moveit humble, (2.5.4)

[samialperen]

I find another issue stating that foxy has better performance than humble when it comes to planning times, but I could not test it on my system since I have too many dependencies for humble, I can not change it to foxy easily

[samialperen]

@azalutsky you were right. Somehow, there is a package collision even if ros2 pkg prefix shows the right package. After applying the fix manually, planning time reduced from 600 ms to 170ms.