dtu_smr_wptnaw: Added stage simulation support, preparing for test on the SMR robot.