Revision history [back]

The first thing to do with any problem like this is you print out the values you're working with on the terminal so you can check if they look sensible. I would check are the values of current_x and current_y first, you're not checking that the call back has been called before using them, so they may well be huge uninitialized values in the billions. You may want to add a spin loop waiting for this call back before starting your main loop.

Also atan2 returns a value in radians, so you don't need to covert it.

Finally if that doesn't fix it then print out the whole value of goal just before you send it.