[JTC] Add console output for tolerance checks

[christophfroehlich]

This PR enables debug output from the tolerance checks, e.g.:

[gzserver-1] [ERROR] [1702714807.880348311] [tolerances]: State tolerances failed for joint 2:
[gzserver-1] [ERROR] [1702714807.880401620] [tolerances]: Position Error: 0.020046, Position Tolerance: 0.010000
[gzserver-1] [WARN] [1702714807.880539038] [trajectory_controllers]: Aborted due goal_time_tolerance exceeding by 1.010000 seconds

[saikishor]

@christophfroehlich why not print it all the time?. What you are printing are errors not the debug part of logging. Moreover, you can set it to always true and use THROTTLE instead of the basic one. I think this way you shouldn't have many problems.

[christophfroehlich]

@christophfroehlich why not print it all the time?. What you are printing are errors not the debug part of logging. Moreover, you can set it to always true and use THROTTLE instead of the basic one. I think this way you shouldn't have many problems.

Throttling would be an option, but I don't know what is a typical goal_time tolerance: At which interval should the throttling run?

[saikishor]

Throttling would be an option, but I don't know what is a typical goal_time tolerance: At which interval should the throttling run?

I would set it to half a second/ a second or so. I think that should be ok in general right?

[codecov]

Codecov Report

Merging #932 (2a858a1) into master (13a43c3) will increase coverage by 0.07%.
The diff coverage is 14.28%.

Additional details and impacted files

@@            Coverage Diff             @@
##           master     #932      +/-   ##
==========================================
+ Coverage   47.17%   47.25%   +0.07%     
==========================================
  Files          41       41              
  Lines        3862     3862              
  Branches     1816     1816              
==========================================
+ Hits         1822     1825       +3     
+ Misses        786      777       -9     
- Partials     1254     1260       +6     

Flag	Coverage Δ
unittests	47.25% <14.28%> (+0.07%)	⬆️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files	Coverage Δ
...include/joint_trajectory_controller/tolerances.hpp	55.00% <0.00%> (+10.00%)	⬆️
...ory_controller/src/joint_trajectory_controller.cpp	48.54% <16.66%> (-0.14%)	⬇️

[christophfroehlich]

but with throtteling and the current method, one probably wouldn't see the output of the last check which is actually cancelling the goal.

[christophfroehlich]

Throttling isn't possible, because it is inside a loop: I don't know a way to "reset" the macro-throttle for a different joint-index.
@saikishor what about this? It should be now printed only once in both checks:

• with state_tolerance the variable tolerance_violated_while_moving will abort the goal after the loop
• If default_tolerances_.goal_time_tolerance is true and the check fails within_goal_time, it will be called again with show_errors=true and the goal will be aborted after the loop

[saikishor]

This looks good to me!