Add SpeedController nodes to adjust replanning rate according to speed (#1744)

* Add SpeedController decorator node and update default BTs

* Fix lint errors

* Add OdomSmoother class and fix SpeedController to use smoothed velocity

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Add duration input port to tree nodes xml for groot

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Revert BT images

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Address reviewer's comments

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Throw BT exception when rate <= 0

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Add simple behavior tree using speed controller

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Set default value to maximum rate on IDLE

* Add test for speed controller

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Use smart pointers in tests

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Fix test

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Move speed controller header file

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Update speed controller to reset only on new goal

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Fix test

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Add params to parameter list

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Fix memory leak

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>