Hello, i was trying to develop the bug 0 algorithm for a robot in ROS (to be more exact with Gazebo). First I made a code to move around walls and now I am trying to make a code to move from one point to another (asked in the terminal) using a state machine, but it is not working.

I have 3 states: rotate(state 0), go_forward(state 1) and stop(state 2). But it doesnt change the state, the robot only makes what he is told with the state it begins the code with. Here I leave the code I´ve got, so if anybody has a clue on whats wrong with it I would be very glad as I have been for 3 days trying to fix it. Thank you very much in advance. C:\fakepath\irpunto (2).py

By the way, sorry some words are in Spanish. Distancia = Distance, Angulo = Angle, Posicion = Position