Buggy code:
public class GlobalNavigation implements NodeMain { private Publisher < GUIRectMsg > guiRectPub ; private Publisher < GUIPolyMsg > guiPolyPub ; private Publisher < GUIEraseMsg > guiErasePub ; private Publisher < GUIPointMsg > guiPtPub ; private Subscriber < OdometryMsg > odometrySub ; private Publisher < MotionMsg > motionPub ; private ColorMsg redMsg ; private ColorMsg greenMsg ; private ColorMsg blueMsg ; private ColorMsg blackMsg ; private List < Point2D . Double > waypoints ; private int currWaypt = 0 ; private String mapFileName ; private PolygonMap polyMap ; private CSpace cSpace ; private MotionPlanner motionPlanner ; public double robotX = 0.0 ; public double robotY = 0.0 ; public double robotTheta = 0.0 ; private double xShift = 0.0 ; private double yShift = 0.0 ; private double thetaShift = 0.0 ; private final double WAYPT_TOL = 0.05 ; private final double WAYPT_TOL_THETA = 0.1 ; double FWD_GAIN = 1 ; double ROT_GAIN = 0.2 ; private boolean navWaypts = false ; private boolean atGoal = false ; private boolean debug = false ; private boolean wayptNav_debug = false ; private boolean odometry_debug = false ; public GlobalNavigation ( ) { } @ Override public void onStart ( Node node ) { } private void wayptNav ( ) { <START_BUG> Point2D . Double wayPoint ; <END_BUG> if ( ( currWaypt ) < ( waypoints . size ( ) ) ) { wayPoint = waypoints . get ( currWaypt ) ; } double currX = robotX ; double currY = robotY ; double currTheta = robotTheta ; if ( ( wayptNav_debug ) == true ) { System . out . println ( ( "currTheta:<seq2seq4repair_space>" + currTheta ) ) ; } double xError = ( wayPoint . getX ( ) ) - currX ; double yError = ( wayPoint . getY ( ) ) - currY ; double thetaToPoint = Math . atan2 ( yError , xError ) ; if ( thetaToPoint < 0 ) { thetaToPoint += 2 * ( Math . PI ) ; } if ( ( wayptNav_debug ) == true ) { System . out . println ( ( "thetaToPoint:<seq2seq4repair_space>" + thetaToPoint ) ) ; } double thetaError = currTheta - thetaToPoint ; if ( ( wayptNav_debug ) == true ) { System . out . println ( ( "Theta<seq2seq4repair_space>Error:<seq2seq4repair_space>" + thetaError ) ) ; } if ( thetaError < ( - ( Math . PI ) ) ) { thetaError += 2 * ( Math . PI ) ; } else if ( thetaError > ( Math . PI ) ) { thetaError -= 2 * ( Math . PI ) ; } if ( ( wayptNav_debug ) == true ) { System . out . println ( ( "Theta<seq2seq4repair_space>Error<seq2seq4repair_space>after<seq2seq4repair_space>correction:<seq2seq4repair_space>" + thetaError ) ) ; } MotionMsg msg = new MotionMsg ( ) ; if ( ( Math . abs ( thetaError ) ) > ( WAYPT_TOL_THETA ) ) { msg . rotationalVelocity = - ( Math . min ( ( ( ROT_GAIN ) * thetaError ) , 0.25 ) ) ; msg . translationalVelocity = 0.0 ; } else if ( ( xError > ( WAYPT_TOL ) ) || ( yError > ( WAYPT_TOL ) ) ) { msg . translationalVelocity = Math . min ( ( ( FWD_GAIN ) * ( motionPlanner . getDist ( 0.0 , 0.0 , xError , yError ) ) ) , 0.5 ) ; msg . rotationalVelocity = 0 ; } else { msg . translationalVelocity = 0.0 ; msg . rotationalVelocity = 0.0 ; if ( ( currWaypt ) < ( waypoints . size ( ) ) ) { int way = ( currWaypt ) + 1 ; System . out . println ( ( ( ( ( ( ( ( ( ( ( ( ( "WAYPOINT:<seq2seq4repair_space>" + way ) + "<seq2seq4repair_space>REACHED<seq2seq4repair_space>at<seq2seq4repair_space>X:<seq2seq4repair_space>" ) + ( wayPoint . getX ( ) ) ) + "<seq2seq4repair_space>Y:<seq2seq4repair_space>" ) + ( wayPoint . getY ( ) ) ) + "<seq2seq4repair_space>ROBOT-X:" ) + currX ) + "<seq2seq4repair_space>ROBOT-Y:" ) + currY ) + "<seq2seq4repair_space>out<seq2seq4repair_space>of<seq2seq4repair_space>" ) + ( waypoints . size ( ) ) ) + "<seq2seq4repair_space>waypoints." ) ) ; currWaypt += 1 ; } else { atGoal = true ; System . out . println ( "At<seq2seq4repair_space>the<seq2seq4repair_space>goal!" ) ; } } motionPub . publish ( msg ) ; if ( ( wayptNav_debug ) == true ) { System . out . println ( ( "Trans<seq2seq4repair_space>Vel:<seq2seq4repair_space>" + ( msg . translationalVelocity ) ) ) ; System . out . println ( ( "Rot<seq2seq4repair_space>Vel:<seq2seq4repair_space>" + ( msg . rotationalVelocity ) ) ) ; } } private void outputPath ( List < Point2D . Double > points , Color color ) { } private void testConvexHull ( ) { } private void displayMap ( ) { } private void displayMapCSpace ( ) { } public void fillPointMsg ( GUIPointMsg msg , Point2D . Double point , Color color ) { } public static void fillRectMsg ( GUIRectMsg msg , Rectangle2D . Double r , Color c , boolean filled ) { } public static void fillPolyMsg ( GUIPolyMsg msg , PolygonObstacle obstacle , Color c , boolean filled , boolean closed ) { } @ Override public void onShutdown ( Node node ) { } @ Override public void onShutdownComplete ( Node node ) { } @ Override public GraphName getDefaultNodeName ( ) { } No newline ; at end ; of file ; }

Fix:
Point2D . Double wayPoint = null ;