PS: add new tests for object pose handling

[v4hn]

with the transition to the new CollisionObject::pose field.

One test currently fails.
@felixvd, Could you please address this together with the other issues I raised in
#2037 (review) ?

[felixvd]

I already see some tests failing, but maybe you could have a look anyway. I think these commits might cover the gist of it.

[v4hn]

CI fails with timeouts, so some test runs into a deadlock with your change.

[felixvd]

If the object consists of a single shape, the object pose should be set to that shape's pose, and the primitive pose to identity. Otherwise, there is a regression for scenes that were built in a scene-graph-like manner by referring to previously placed objects.

It also looks nicer when you visualize the object pose in TF.

[codecov]

Codecov Report

Merging #2816 (70fd059) into master (7cea1f2) will increase coverage by 0.07%.
The diff coverage is 84.32%.

❗ Current head 70fd059 differs from pull request most recent head 86ac716. Consider uploading reports for the commit 86ac716 to get more accurate results

@@            Coverage Diff             @@
##           master    #2816      +/-   ##
==========================================
+ Coverage   60.81%   60.88%   +0.07%     
==========================================
  Files         366      366              
  Lines       31717    31705      -12     
==========================================
+ Hits        19285    19299      +14     
+ Misses      12432    12406      -26     

Impacted Files	Coverage Δ
...ene/include/moveit/planning_scene/planning_scene.h	79.37% <ø> (ø)
moveit_core/planning_scene/src/planning_scene.cpp	63.14% <84.32%> (+1.32%)	⬆️
...g_scene_interface/src/planning_scene_interface.cpp	47.46% <0.00%> (-1.12%)	⬇️
...nning_scene_monitor/src/planning_scene_monitor.cpp	68.13% <0.00%> (-0.13%)	⬇️
...strial_motion_planner/src/trajectory_generator.cpp	100.00% <0.00%> (ø)
...z_industrial_motion_planner/trajectory_generator.h	100.00% <0.00%> (ø)
...meterization/work_space/pose_model_state_space.cpp	83.02% <0.00%> (+2.52%)	⬆️
.../ompl_interface/src/detail/constrained_sampler.cpp	59.46% <0.00%> (+16.22%)	⬆️

---

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 7cea1f2...86ac716. Read the comment docs.

[felixvd]

CI passes with the new tests, and the change in this PR fixes a regression, so it would be great if someone could take a look soon. It also makes the PlanningScene code clearer, which I like a lot.

I left out this change addressing the copy because it's only a (suspected) performance improvement and there's a bug left in it. If someone sees it, let me know and I'll fix it up (or do it yourself and submit the PR, I'm not your boss).

[v4hn]

Thanks for addressing this @felixvd ! This test is crucial to help people with the migration to the new object poses. 🏅

[felixvd]

Thanks for catching it and adding the tests <3