Fix counterTD (Differentiation is discrete)

[TakaakiNumai]

Abstract

The value of velocity is discrete because the low resolution angle is differentiated in a short time.
Extend the sampling period when the change in angle is small and smooth the velocity values.

Reference:https://jsdkk.com/blog/mugita/?p=458

Purpose

To prevent PID control from vibrating

Implementation Details

[TakaakiNumai]

Implement two functions counterTDRaw and counterTDVariableWindow to be able to switch between them with rosparam

[TakaakiNumai]

This program is converting a number subtracted by uint16_t to a double, so -1 is 65535.
It seems that the gain was diverging each time the counter increased by 65536.

vesc/vesc_hw_interface/src/vesc_wheel_controller.cpp

Lines 169 to 170 in d96dcc2

 counter_td_tmp_[0] = static_cast<double>(counter_changed_log_[0][0] - counter_changed_log_[1][0]) / 
                      static_cast<double>(counter_changed_log_[0][1]);