Weird timings with multi-threaded executors and callback groups

I have encountered a weird issue where running my code in Obelisk (i.e. with my whole robot stack running) causes the code to be much less consistent timing-wise. To clarify, I am NOT suggesting that the ROS2 timer is bad, but that when I am within a callback executing computationally intensive code (that has a nominal run time of about 10ms), the runtime has high variance.

The callback is within a separate callback group and thus should be executing parallel to all the other callbacks in the node. I have confirmed this by plotting some other outputs and verifying that the other callbacks happen in parallel.

So there are a few options:

The ROS2 executors is causing some weird performance and potentially pausing my thread for some other thread to execute
With so many other processes running, maybe I am getting bad cache hits (i.e. lots of misses) which causes a significant amount of overhead. Also, this will be random.
I am getting stalled waiting on a OS level call (maybe stalled waiting for a malloc or free)
There is a non-ROS thread that is preempting my computation.

To help eliminate some of these options, I booted up my robot stack (simulator, estimator, controller) and stopped the MPC computation (which is the computation in question). Then I ran my MPC separately (i.e. with no ROS or obelisk) and the timings were still consistent.

This would seem to imply that options 2-4 are non-issues, as those would be present no matter where I was running my MPC. So for now I am investigating the first option.

There appear to be a few options like:

https://github.com/ros2/examples/tree/humble/rclcpp/executors/cbg_executor
Using a real time kernel

Both of these options are kinda painful, and using a real time kernel come with other overhead, like dealing with memory allocation correctly.

I will update this thread with findings as I get them.

For maximum information, here are my timings. On my local machine, without ROS/Obelisk:

    =============================================================================================================================================== MPC Statistics ===============================================================================================================================================
                  Solve #             Solve Status                Time (ms)        Constr. Time (ms)           Cost Time (ms)             LS Time (ms)         d-norm (post LS)                    Alpha           LS Termination    Constr. Vio. (Scaled)           Cost (post LS)         QP Cost (pre LS)
                        0           Max Iterations                   12.859                    4.411                    0.663                    0.708                   17.739                        1          Constraint Vio.                0.0222591                0.0224777                0.0224777
                        1           Max Iterations                    8.914                    1.923                    0.381                    1.401                  1.51542                     0.25          Constraint Vio.                0.0117601                0.0221844              -0.00150576
                        2           Max Iterations                    9.941                    1.895                    0.375                    2.455                  2.13415                  0.03125          Constraint Vio.                0.0113445                0.0215505               -0.0106802
                        3           Max Iterations                   10.237                    1.851                    0.402                    2.799                  1.03811                 0.015625          Constraint Vio.                 0.011507                0.0212508               -0.0100351
                        4           Max Iterations                   10.694                    1.931                    0.376                    3.152                 0.512731                0.0078125          Constraint Vio.                0.0115939                 0.021105              -0.00973054
                        5           Max Iterations                   10.597                    1.841                    0.375                    3.146                 0.509624                0.0078125          Constraint Vio.                0.0116391                0.0209614              -0.00958232
                        6           Max Iterations                   10.911                    1.823                    0.377                    3.501                 0.253128               0.00390625          Constraint Vio.                0.0116845                0.0208907              -0.00943604
                        7           Max Iterations                   11.285                    1.841                    0.376                    3.851                 0.126183               0.00195312          Constraint Vio.                0.0117075                0.0208555              -0.00936404
                        8           Max Iterations                   11.292                    1.838                    0.377                    3.845                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932828
                        9           Max Iterations                   11.289                    1.827                    0.376                    3.844                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
                       10           Max Iterations                   11.264                    1.824                    0.377                    3.851                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
                       11           Max Iterations                   11.293                    1.829                    0.376                    3.852                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
                       12           Max Iterations                   11.313                    1.822                    0.384                    3.855                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
                       13           Max Iterations                   11.296                    1.819                    0.377                    3.856                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
                       14           Max Iterations                   11.311                    1.813                    0.377                    3.855                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
                       15           Max Iterations                   11.293                    1.826                    0.377                    3.846                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
                       16           Max Iterations                   11.328                    1.818                    0.377                     3.85                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
                       17           Max Iterations                   11.328                    1.831                    0.376                     3.85                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
                       18           Max Iterations                   11.355                    1.841                    0.381                    3.858                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
                       19           Max Iterations                   11.348                     1.84                    0.375                    3.846                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
                       20           Max Iterations                   11.339                    1.832                    0.377                    3.851                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
                       21           Max Iterations                    8.186                    1.836                    0.374                    0.708                  64.9059                        1          Constraint Vio.                0.0262206                0.0122762              -0.00926403
                       22           Max Iterations                    8.229                    1.829                    0.375                    0.706                  12.8099                        1          Constraint Vio.                0.0144647                0.0124934             -5.45714e-05
                       23           Max Iterations                    8.251                    1.831                    0.375                    0.702                  43.8581                        1          Constraint Vio.                 0.134022                0.0124083             -0.000109118
                       24           Max Iterations                    8.245                    1.832                    0.376                    0.702                  33.1219                        1          Constraint Vio.                  0.13269                0.0123837             -9.19499e-06
                       25           Max Iterations                    8.302                    1.831                    0.374                    0.704                  38.3871                        1          Constraint Vio.                 0.135461                0.0123458             -1.22958e-05
                       26           Max Iterations                    8.336                    1.829                    0.372                    0.705                  41.1221                        1          Constraint Vio.                 0.128356                0.0123045             -1.03386e-05
                       27           Max Iterations                    8.356                    1.827                    0.376                    0.706                  44.6028                        1          Constraint Vio.                  0.12366                0.0122592             -1.31442e-05
                       28           Max Iterations                    8.273                    1.826                    0.373                    0.704                  48.4862                        1          Constraint Vio.                 0.120451                0.0122123             -1.59045e-05
                       29           Max Iterations                    8.247                    1.834                    0.372                    0.702                  52.6863                        1          Constraint Vio.                 0.118121                0.0121652             -1.88787e-05
                       30           Max Iterations                    8.286                    1.855                    0.373                    0.705                  57.1902                        1          Constraint Vio.                 0.116211                0.0121189             -2.22624e-05
                       31           Max Iterations                     8.26                     1.83                    0.374                    0.707                  61.9063                        1          Constraint Vio.                 0.114325                0.0120739             -2.60359e-05
                       32           Max Iterations                    8.257                     1.83                    0.373                    0.704                   57.491                        1          Constraint Vio.                 0.115088                0.0120349             -2.58544e-05
                       33           Max Iterations                    8.257                    1.832                    0.377                    0.708                  40.5459                        1          Constraint Vio.                 0.113996                0.0120131             -1.50764e-05
                       34           Max Iterations                    8.301                    1.853                    0.375                    0.706                   29.402                        1          Constraint Vio.                 0.112967                0.0120037             -9.70861e-06
                       35           Max Iterations                    8.276                    1.839                    0.375                    0.703                  21.0494                        1          Constraint Vio.                 0.110844                 0.012005             -6.16991e-06
                       36           Max Iterations                    8.256                    1.834                    0.373                    0.701                  15.3642                        1          Constraint Vio.                  0.10874                0.0120155              -3.9107e-06
                       37           Max Iterations                    8.278                    1.837                    0.376                    0.706                  11.4684                        1          Constraint Vio.                 0.106735                0.0120339               -2.497e-06
                       38           Max Iterations                    8.234                     1.83                    0.374                    0.706                  8.61601                        1          Constraint Vio.                 0.104988                0.0120591             -1.66126e-06
                       39           Max Iterations                      8.2                    1.825                    0.373                    0.704                   6.9739                        1          Constraint Vio.                 0.103721                0.0120899             -1.26765e-06
                       40           Max Iterations                    8.266                    1.876                    0.374                    0.704                  5.09747                        1          Constraint Vio.                  0.10272                0.0121253             -1.07984e-06

Running the came code in the obelisk stack:

[ll_controller-2] =============================================================================================================================================== MPC Statistics ===============================================================================================================================================
[ll_controller-2]                   Solve #             Solve Status                Time (ms)        Constr. Time (ms)           Cost Time (ms)             LS Time (ms)         d-norm (post LS)                    Alpha           LS Termination    Constr. Vio. (Scaled)           Cost (post LS)         QP Cost (pre LS)
[ll_controller-2]                         0           Max Iterations                   10.504                    2.256                    0.413                     0.75                   17.739                        1          Constraint Vio.                0.0222591                0.0224777                0.0224777
[ll_controller-2]                         1           Max Iterations                    9.237                    2.023                    0.397                    1.432                  1.51542                     0.25          Constraint Vio.                0.0117601                0.0221844              -0.00150576
[ll_controller-2]                         2           Max Iterations                   10.206                    2.004                    0.378                    2.504                  2.13415                  0.03125          Constraint Vio.                0.0113445                0.0215505               -0.0106802
[ll_controller-2]                         3           Max Iterations                   10.398                    1.919                    0.383                    2.868                  1.03811                 0.015625          Constraint Vio.                 0.011507                0.0212508               -0.0100351
[ll_controller-2]                         4           Max Iterations                   10.916                    2.026                    0.376                    3.213                 0.512731                0.0078125          Constraint Vio.                0.0115939                 0.021105              -0.00973054
[ll_controller-2]                         5           Max Iterations                   10.699                    1.846                    0.368                    3.203                 0.509624                0.0078125          Constraint Vio.                0.0116391                0.0209614              -0.00958232
[ll_controller-2]                         6           Max Iterations                   11.166                    2.007                    0.379                    3.574                 0.253128               0.00390625          Constraint Vio.                0.0116845                0.0208907              -0.00943604
[ll_controller-2]                         7           Max Iterations                   11.405                    1.885                     0.37                    3.907                 0.126183               0.00195312          Constraint Vio.                0.0117075                0.0208555              -0.00936404
[ll_controller-2]                         8           Max Iterations                   11.522                    1.972                    0.364                    3.921                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932828
[ll_controller-2]                         9           Max Iterations                    11.39                    1.872                    0.377                    3.919                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
[ll_controller-2]                        10           Max Iterations                   11.609                    2.014                    0.374                    3.924                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
[ll_controller-2]                        11           Max Iterations                   11.486                     1.87                    0.373                    3.912                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
[ll_controller-2]                        12           Max Iterations                   11.661                    2.026                    0.386                    3.929                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
[ll_controller-2]                        13           Max Iterations                   11.671                    1.917                    0.385                    3.942                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
[ll_controller-2]                        14           Max Iterations                   11.632                    2.014                    0.383                    3.936                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
[ll_controller-2]                        15           Max Iterations                   11.429                    1.888                     0.38                    3.934                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
[ll_controller-2]                        16           Max Iterations                   11.495                    1.973                    0.368                    3.929                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
[ll_controller-2]                        17           Max Iterations                   11.491                    1.875                    0.387                    3.911                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
[ll_controller-2]                        18           Max Iterations                   11.741                    2.169                    0.373                     3.93                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
[ll_controller-2]                        19           Max Iterations                   11.419                    1.921                    0.379                     3.91                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
[ll_controller-2]                        20           Max Iterations                   11.521                    2.013                    0.372                    3.923                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
[ll_controller-2]                        21           Max Iterations                   19.609                    9.153                    1.035                    0.758                  109.426                        1          Constraint Vio.                0.0738822                  0.45633              -0.00193624
[ll_controller-2]                        22           Max Iterations                    8.799                    2.098                    0.376                    0.736                  6.67122                        1           Cost & Constr.               0.00863229                 0.454741             -0.000660267
[ll_controller-2]                        23           Max Iterations                   11.447                    3.501                    0.775                    0.748                  34.1119                        1          Constraint Vio.                0.0160506                   1.1797              -0.00139687
[ll_controller-2]                        24           Max Iterations                   13.346                    3.426                    0.761                    0.725                  30.8145                        1          Constraint Vio.                0.0209005                 0.612699               0.00990061
[ll_controller-2]                        25           Max Iterations                   10.781                    3.419                    0.613                    0.835                  24.6192                        1          Constraint Vio.                0.0132674                 0.234217             -4.49046e-05
[ll_controller-2]                        26           Max Iterations                   13.772                    3.439                    0.601                     1.44                  40.8118                        1          Constraint Vio.                0.0112539                0.0458397              -0.00383605
[ll_controller-2]                        27           Max Iterations                   15.813                    3.325                    0.603                    1.013                  35.0761                        1          Constraint Vio.                  0.02545                0.0251804              -0.00561046
[ll_controller-2]                        28           Max Iterations                   11.888                    2.563                    0.413                    1.086                  21.9036                        1          Constraint Vio.                0.0142333                 0.036332              0.000342503
[ll_controller-2]                        29           Max Iterations                   13.608                    3.637                    0.758                    0.735                  13.7783                        1           Cost & Constr.                0.0060477                0.0390554             -0.000203735
[ll_controller-2]                        30           Max Iterations                    9.865                    3.204                    0.374                    0.782                  5.59209                        1           Cost & Constr.               0.00447015                0.0397005             -4.34257e-05
[ll_controller-2]                        31           Max Iterations                    8.803                    1.941                    0.368                    0.804                  8.56868                        1           Cost & Constr.               0.00476603                0.0721889              0.000378716
[ll_controller-2]                        32           Max Iterations                   14.348                    3.434                    0.723                    1.084                  13.1031                        1           Cost & Constr.               0.00451981                0.0452681             -0.000144914
[ll_controller-2]                        33           Max Iterations                   17.895                    4.486                    0.716                    1.554                  7.58895                        1           Cost & Constr.               0.00458032                0.0476426              9.33807e-05
[ll_controller-2]                        34           Max Iterations                   32.195                    3.452                     0.82                    3.803                  8.47886                        1           Cost & Constr.               0.00515953                 0.050264              7.35116e-05
[ll_controller-2]                        35           Max Iterations                   13.915                    6.575                    0.589                     0.76                  27.7493                        1           Cost & Constr.               0.00968959                0.0976939              0.000687243
[ll_controller-2]                        36           Max Iterations                     9.89                    3.209                    0.377                    0.729                  26.4951                        1           Cost & Constr.               0.00797128                0.0710516              0.000379841
[ll_controller-2]                        37           Max Iterations                     8.47                    1.971                    0.393                    0.707                  9.34709                        1           Cost & Constr.               0.00599771                0.0718712             -0.000319661
[ll_controller-2]                        38           Max Iterations                   11.242                    2.364                    0.367                    0.905                   10.797                        1           Cost & Constr.                0.0052209                0.0410978              -0.00013881
[ll_controller-2]                        39           Max Iterations                   12.372                    2.774                      0.6                    0.761                  11.0887                        1           Cost & Constr.               0.00853592                0.0331956             -4.83322e-05
[ll_controller-2]                        40           Max Iterations                   12.366                    2.401                    0.374                    4.024                        0                        0              Alpha small                0.0130373                0.0331994             -0.000165711

Spinning up another thread from withing the ROS node and running the same code is much better. See the below timings. This would further confirm that the ROS executor is using my thread for other tasks. So there are a few options here:

Investigate the method linked above where they use a different executor for each priority task.
Further investigate a real time kernel
Generalize the solution of spinning up another thread from within the node and write a nice wrapper where we can maybe deal with the ros parameters (like loop timings) and the thread waiting.

[ll_controller-2] =============================================================================================================================================== MPC Statistics ===============================================================================================================================================
[ll_controller-2]                   Solve #             Solve Status                Time (ms)        Constr. Time (ms)           Cost Time (ms)             LS Time (ms)         d-norm (post LS)                    Alpha           LS Termination    Constr. Vio. (Scaled)           Cost (post LS)         QP Cost (pre LS)
[ll_controller-2]                         0           Max Iterations                   10.432                    2.721                    0.396                    0.745                   17.739                        1          Constraint Vio.                0.0222591                0.0224777                0.0224777
[ll_controller-2]                         1           Max Iterations                    9.287                    2.065                    0.381                    1.425                  1.51542                     0.25          Constraint Vio.                0.0117601                0.0221844              -0.00150576
[ll_controller-2]                         2           Max Iterations                   10.494                    2.103                    0.379                    2.568                  2.13415                  0.03125          Constraint Vio.                0.0113445                0.0215505               -0.0106802
[ll_controller-2]                         3           Max Iterations                   10.389                    1.935                    0.368                    2.802                  1.03811                 0.015625          Constraint Vio.                 0.011507                0.0212508               -0.0100351
[ll_controller-2]                         4           Max Iterations                   10.851                    2.024                    0.368                    3.152                 0.512731                0.0078125          Constraint Vio.                0.0115939                 0.021105              -0.00973054
[ll_controller-2]                         5           Max Iterations                    10.67                    1.906                    0.366                    3.154                 0.509624                0.0078125          Constraint Vio.                0.0116391                0.0209614              -0.00958232
[ll_controller-2]                         6           Max Iterations                   11.105                    2.003                    0.356                    3.519                 0.253128               0.00390625          Constraint Vio.                0.0116845                0.0208907              -0.00943604
[ll_controller-2]                         7           Max Iterations                   11.362                    1.883                    0.364                    3.841                 0.126183               0.00195312          Constraint Vio.                0.0117075                0.0208555              -0.00936404
[ll_controller-2]                         8           Max Iterations                   11.509                    2.008                    0.368                    3.852                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932828
[ll_controller-2]                         9           Max Iterations                   11.346                    1.872                     0.36                    3.854                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
[ll_controller-2]                        10           Max Iterations                   11.438                    2.001                    0.367                    3.836                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
[ll_controller-2]                        11           Max Iterations                   11.295                    1.871                    0.356                    3.844                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
[ll_controller-2]                        12           Max Iterations                   19.381                    3.208                    0.741                    6.488                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
[ll_controller-2]                        13           Max Iterations                   11.976                    2.416                     0.38                    3.856                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
[ll_controller-2]                        14           Max Iterations                   11.538                    2.043                    0.375                    3.841                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
[ll_controller-2]                        15           Max Iterations                   11.368                    1.895                    0.367                    3.839                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
[ll_controller-2]                        16           Max Iterations                     11.4                    1.986                    0.359                    3.835                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
[ll_controller-2]                        17           Max Iterations                   11.344                    1.875                    0.367                    3.835                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
[ll_controller-2]                        18           Max Iterations                    11.46                    1.987                    0.359                    3.846                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
[ll_controller-2]                        19           Max Iterations                   11.383                    1.874                    0.362                    3.842                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
[ll_controller-2]                        20           Max Iterations                    11.42                    1.982                    0.354                    3.844                        0                        0              Alpha small                0.0117191                0.0208555              -0.00932836
[ll_controller-2]                        21           Max Iterations                    10.12                    3.457                     0.39                    0.732                  109.426                        1          Constraint Vio.                0.0738822                  0.45633              -0.00193624
[ll_controller-2]                        22           Max Iterations                    9.046                    2.119                    0.382                    0.761                  6.67122                        1           Cost & Constr.               0.00863229                 0.454741             -0.000660267
[ll_controller-2]                        23           Max Iterations                    8.571                    1.894                    0.372                    0.731                  57.8571                        1          Constraint Vio.                0.0412314                  1.21669               0.00451252
[ll_controller-2]                        24           Max Iterations                    9.267                    2.187                    0.373                    1.176                  28.0069                        1          Constraint Vio.                0.0312112                0.0640833                0.0112125
[ll_controller-2]                        25           Max Iterations                   11.869                    3.125                    0.745                    0.735                  76.7305                        1          Constraint Vio.                0.0463719                0.0287299               -0.0157606
[ll_controller-2]                        26           Max Iterations                    8.657                    2.022                    0.396                    0.725                  36.0398                        1          Constraint Vio.                0.0169596                0.0553494             -0.000423959
[ll_controller-2]                        27           Max Iterations                   11.332                    3.137                    0.636                    0.726                   17.971                        1           Cost & Constr.               0.00726488                0.0735171              0.000409318
[ll_controller-2]                        28           Max Iterations                   13.437                    3.153                    0.736                    0.736                   24.021                        1           Cost & Constr.               0.00770841                 0.146878              0.000686594
[ll_controller-2]                        29           Max Iterations                    9.289                    2.121                    0.497                    0.859                   12.632                        1           Cost & Constr.                0.0065797                 0.160314              0.000761276
[ll_controller-2]                        30           Max Iterations                    9.623                     1.91                    0.373                    1.219                  13.1454                        1           Cost & Constr.               0.00585627                 0.131017              0.000547705
[ll_controller-2]                        31           Max Iterations                   13.696                    3.194                    0.752                    0.729                   11.569                        1           Cost & Constr.               0.00599776                0.0948979               0.00028071
[ll_controller-2]                        32           Max Iterations                    8.616                    1.939                    0.396                    0.728                  15.7652                        1           Cost & Constr.               0.00778837                0.0491175              0.000731325
[ll_controller-2]                        33           Max Iterations                    8.527                    1.912                    0.382                    0.721                  22.7657                        1          Constraint Vio.                0.0155961                0.0313829              -0.00128082
[ll_controller-2]                        34           Max Iterations                    8.978                    2.042                    0.382                    0.738                  58.0606                        1          Constraint Vio.                 0.128504                0.0370216              -0.00094114
[ll_controller-2]                        35           Max Iterations                    8.633                    1.929                    0.381                    0.726                  17.7913                        1          Constraint Vio.                0.0507902                0.0483244              0.000918355
[ll_controller-2]                        36           Max Iterations                    8.869                    1.899                    0.398                    0.895                  13.6334                        1          Constraint Vio.                0.0284923                 0.057063              0.000921084
[ll_controller-2]                        37           Max Iterations                    8.509                    1.902                    0.383                    0.733                  18.9256                        1          Constraint Vio.                0.0230733                0.0465285              0.000533227
[ll_controller-2]                        38           Max Iterations                    8.558                    1.898                    0.388                    0.751                  22.5115                        1          Constraint Vio.                0.0597208                0.0443469              -0.00135158
[ll_controller-2]                        39           Max Iterations                    8.879                    1.964                    0.404                    0.756                   35.068                        1          Constraint Vio.                 0.120052                0.0550595              0.000192447
[ll_controller-2]                        40           Max Iterations                    8.752                    1.921                    0.384                     0.74                   14.371                        1          Constraint Vio.                0.0529454                0.0868951               0.00150442

Caltech-AMBER / obelisk

Weird timings with multi-threaded executors and callback groups #115