Plane extraction updated

Moved the PCL test into a proper node. Function for extracting
multiple planes from the cloud in progress. Currently extracts the
dominant plane of the cloud.