Reliance on synchronous service calls within toolpath planner setup callback

[grdwyer]

Setup callback uses multiple service calls for setting up the planning environment. Main issue is modifying the ACM which needs to be synchronous, but also the planning relies on the move_group interface such as getting the current state which are service calls.

Potential solutions

• Splitting the setup calls into separate smaller callbacks triggered by the behaviour trees
  • Pro is potentially a more versatile setup that can be modified without compiling
  • Con increased complexity and the chance of node creep
• Using threads in the service call back to startup the thread with the setup tasks then an additional service call to check if the planning operation has been successful

[grdwyer]

went for the btree version and merged in #41