only some small additions for data recording. safecontroller is also writing to bagfile allowing to find the controller mistake.