Enable and disable robot safely by modifying __init__ and adding on_shutdown callback