More Tests to Planning

[mistrynick]

In src/kestrel_planning/test/test_waypoints.py
create more tests that validate these concepts:

1. multiple replans (auto replan without manually retrigger)
   a. move goal
   b. move goal and start
   c. move obstacles
   d. move obstacles and move goal and start
2. metrics
   a. time taken to compute a path
   b. time taken to replan
3. general improvements
   a. for all tests, check if the returned path doesnt contain any obstacles.