[Simulation] add ESC_Telemetry

[Li-Jinjie]

What is this

Our real-world system has (partly, mainly beetle) supported ESC telemetry, but the simulation does not support. Thus, this PR adds a publisher and two msgs to pub the ESC data.

Details

Add a "krpm_rate" to yaml config with (kRPM)^2/N unit.

I find the original equation for rotor speed is "force_ * speed_rate_", which is incorrect, since the thrust is proportional to the square of the motor speed. However, when I change it to the correct equation, the robot may diverge in simulation and I don't know why.

Hence, I add an additional topic to convert the thrust directly to RPM.

TODO

• Currently, ESC Telemetry msg is unable to contain any number of motors. Need to change this msg in the future.
• The ESC Telemetry contains: temperature, voltage, current, consumption, rpm, and crc_error, which might be too many and require too much communication bandwidth. In the future, we may only need rpm for high-frequency bidirectional dshot.
• The measure data from ESC is actually eRPM: (number of poles /2 * RPMs) = eRPM. The number of poles (magnets) is related to the specific motor type and should be configured in yaml config.