Corrected library name bug
Conversion for stage simulation
Implemented test behaviour that adds a waypoit 2 mtrs ahead of current position