Ensure mesh is convexified before adding to environment

[marip8]

The scan mesh being added to the planning environment is generally not convex, but loading it directly from file as a convex mesh implies to the collision checker that all of the vertices in the mesh represent a convex hull. AFAIK collision checking still occurs as if this mesh were a convex hull, but if the mesh is not a convex hull and has many more vertices than its convex hull (which is typical), then collision checking takes much longer. For our use case, our mesh had 150k vertices, but its convex hull had only 200 vertices. Without actually creating the convex hull of the mesh, it took several minutes (with a debug build) to load the mesh into a Bullet collision object and do collision checking, whereas the time with the convex hull was on the order of seconds.

[marrts]

This will definitely be nice, would it make more sense to do a convex decomposition on the mesh first and add all the convex hulls?

[marip8]

This will definitely be nice, would it make more sense to do a convex decomposition on the mesh first and add all the convex hulls?

Maybe. It's hard to tell without any visualization of what's created. I tried convex decomposition at Automate for the composite mold part, and the planning still failed due to collision. I think this PR is an okay short-term solution. Maybe with a later version of tesseract it will be easier to visualize these collision models and then we could think more about convex decomposition

[marrts]

That makes sense, visualizing the decomposition is extremely useful and it would need to be tuned for the application.