Many tweaks. Launchfile works again. More use of affine3d. Tf publisher for world->ground based on trajectory.