search

nobleo / path_tracking_pid

Path Tracking PID offers a tuneable PID control loop, decouling steering and forward velocity
Apache License 2.0
129 stars 37 forks source link

publishing nan velocity due to infinite inverse radius assignment #158

Open talhaaliarslan opened 1 year ago

talhaaliarslan commented 1 year ago

Came across this corner case where angular field in the command velocity is set to nan which may cause erratic movements on a robot if not handled on the receiving side. The nan value is coming from feedforward_ang_ component of the controller, which uses inverse turning radius as:

https://github.com/nobleo/path_tracking_pid/blob/750f8c1a35f6f78771bb16b15b07015fef4b3a6d/src/controller.cpp#L559

The part where inverse turn radii are computed have a return value of infinity when delta is below an epsilon specified which includes a zero value too :

https://github.com/nobleo/path_tracking_pid/blob/750f8c1a35f6f78771bb16b15b07015fef4b3a6d/src/calculations.cpp#L45-L60

This is causing an instance of nan command velocity (angular) in the controller which stays permanent even if inverse radii return back to finite values.

Timple commented 1 year ago

Ah, good catch!

Did you have fix for this already in mind? Otherwise we can look into it after next week.

DemyKremers commented 1 year ago

Hey @Timple , Talha and I fixed (and tested) this by replacing line 57 in calculations.cpp: return std::numeric_limits<double>::infinity();

by:
return std::numeric_limits<double>::max();

Hope this helps.