Simulation testing for FIRA pathplanner