Construct plan request messy and unstructured

[grdwyer]

construct_plan_request is now over 100 lines long and quite messy should be broken down into sub functions and structured properly.

[grdwyer]

Possible structure

• configuration (loading params and setting the frames)
• initialisation (get the toolpath and transform it)
• compute toolpath (just the actual poses required no planning)
• get approach pose
• get retreat pose
• Plan
  • approach
  • toolpath
  • retreat
• join plans and check

[grdwyer]

41 considerably cleaned up the construct plan request