Splits motion_planning_frame.cpp into different files