Fix/cancel goal

[RichardvdK]

Sorry for the "big diff", but this file needed some love. :) Implement canceling of goals as that was not possible before. (calling .cancel_goal() not on the goal handler is not allowed)

The biggest changes are:

• Fixed the execution and preemption timeout timers
• Made 'outcomes' a param to make it compatible with your state machine
• Updated state names to ROS2 convention (preempt -> cancel)
• added extra intermediate internal state for preemption and used state enum instead of global vars
• added function to wait for cancelation request response with timeout

[martincerven]

What does this accomplish? Are preemptions not working in ROS2?

[Timple]

Nope, preempt timeouts are not functional without this PR.