Add changes made by Andrew and add goal_radius as parameter of drone_nav