Fix std vector insertion (collision distance field)

[andreas-botbuilt]

Description

Calling getCollisionPoints on non_group_attached_body_decomposition (see here) returns iterators to different vectors, thus causing undefined behavior, and resulting in the error:

terminate called after throwing an instance of 'std::length_error'
what(): vector::_M_range_insert

This PR fixes this error.

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

Merging #942 (d6d6eae) into main (c8d3b19) will decrease coverage by 0.02%.
The diff coverage is 0.00%.

@@            Coverage Diff             @@
##             main     #942      +/-   ##
==========================================
- Coverage   57.92%   57.91%   -0.01%     
==========================================
  Files         310      310              
  Lines       26069    26069              
==========================================
- Hits        15098    15094       -4     
- Misses      10971    10975       +4     

Impacted Files	Coverage Δ
...istance_field/src/collision_env_distance_field.cpp	54.16% <0.00%> (ø)
...dl_kinematics_plugin/src/kdl_kinematics_plugin.cpp	74.82% <0.00%> (-1.11%)	⬇️
...nning_scene_monitor/src/planning_scene_monitor.cpp	53.19% <0.00%> (-0.11%)	⬇️

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update c8d3b19...d6d6eae. Read the comment docs.

[JafarAbdi]

This need to be backported to https://github.com/ros-planning/moveit