Planning scene octomap test

[JStech]

Description

Adding TestPlanWithOctomap to moveit_cpp_test.cpp.

This test adds an octomap to the scene and computes a plan. It then clears the octomap and computes the collision distance, which segfaults without #2104 (related: flexible-collision-library/fcl#473).

In comments I've included code that uses PCL to generate an octomap for the test, and then hard-coded the PointCloud2 message, so that PCL needn't be added as a new dependency.

Checklist

• [x] 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
• [x] 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[bot]]

Codecov Report

Merging #2129 into master will decrease coverage by 1.22%. The diff coverage is n/a.

@@            Coverage Diff             @@
##           master    #2129      +/-   ##
==========================================
- Coverage   57.82%   56.59%   -1.23%     
==========================================
  Files         328       31     -297     
  Lines       25700     2841   -22859     
==========================================
- Hits        14861     1608   -13253     
+ Misses      10839     1233    -9606     

Impacted Files	Coverage Δ
...perception/mesh_filter/src/stereo_camera_model.cpp
...tection/include/moveit/collision_detection/world.h
.../moveit/warehouse/trajectory_constraints_storage.h
...eit_experimental/moveit_jog_arm/src/jog_server.cpp
...ntal/moveit_jog_arm/src/collision_check_thread.cpp
...veit_jog_arm/include/moveit_jog_arm/jog_arm_data.h
...ners/ompl/ompl_interface/src/detail/goal_union.cpp
...ce_capability/src/pick_place_action_capability.cpp
...collision_detection/collision_detector_allocator.h
...e/moveit/robot_interaction/kinematic_options_map.h
... and 288 more

---

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 1518cbf...d62d6e8. Read the comment docs.

[JStech]

Yeah, these were the options we considered:

1. Include a bag file somewhere with a point cloud in it,
2. Include the PCL code to generate the point cloud (and add PCL as a dependency), or
3. Just include the hardcoded point cloud message.

I think the current state of things is the best compromise between code simplicity (no PCL dependence) and readability (all the PCL code to generate the message is there, just commented out). I'm open to other suggestions, though.

[Dale-Koenig]

Yeah, these were the options we considered:

1. Include a bag file somewhere with a point cloud in it,

2. Include the PCL code to generate the point cloud (and add PCL as a dependency), or

3. Just include the hardcoded point cloud message.

I think the current state of things is the best compromise between code simplicity (no PCL dependence) and readability (all the PCL code to generate the message is there, just commented out). I'm open to other suggestions, though.

One choice would be to try to fill in the desired pointcloud msg data using a sensor_msgs::PointCloud2Modifier, although it has the downside of being unconventional

[JStech]

Thanks, @Dale-Koenig -- I didn't know about that, but it's probably the best way to do it. I'll convert it, and see if I can figure out this CI failure.

[davetcoleman]

@JStech can you update this to "WIP" since I believe it was never finished? Or put a little more time into it to get it across the line?