Larger chunk of work porting to MoveIt/groovy.
Includes many changes to pose_follower_3d and sbpl_lattice_planner_layer_3d packages.
May be close to compiling but does not include a source for the incoming costmap data.