Buggy code:
public class LocalizationTest implements NodeMain { private Publisher < GUIRectMsg > guiRectPub ; private Publisher < GUIPolyMsg > guiPolyPub ; private Publisher < GUIEraseMsg > guiErasePub ; private Publisher < GUIPointMsg > guiPtPub ; private Publisher < Object > ellipsePub ; private Publisher < Object > stringPub ; private ColorMsg redMsg ; private ColorMsg greenMsg ; private ColorMsg blueMsg ; private ColorMsg blackMsg ; private String mapFileName ; private GrandChallengeMap challengeMap ; private CSpace cSpace ; private ArrayList < ArrayList < PolygonObstacle > > obsCSpaces = new ArrayList < ArrayList < PolygonObstacle > > ( ) ; private RRT rrt ; private List < Point2D . Double > rrtPath ; private ParticleFilter particleFilter ; private Point2D . Double robotStart ; private Point2D . Double robotGoal ; private Point2D . Double prevPt ; private Color lightBlue = new Color ( 115 , 115 , 230 ) ; private Color darkBlue = new Color ( 50 , 40 , 120 ) ; private final int cspaceIndex = 90 ; private final double TOLERANCE = 0.02 ; public LocalizationTest ( ) { } @ Override public void onStart ( Node node ) { stringPub = node . newPublisher ( "gui//String" , "Challenge_msgs//GUIStringMessage" ) ; ellipsePub = node . newPublisher ( "//gui//Ellipse" , "Challenge_msgs//GUIEllipseMessage" ) ; guiRectPub = node . newPublisher ( "gui//Rect" , "lab6_msgs//GUIRectMsg" ) ; guiPolyPub = node . newPublisher ( "gui//Poly" , "lab6_msgs//GUIPolyMsg" ) ; guiErasePub = node . newPublisher ( "gui//Erase" , "lab5_msgs//GUIEraseMsg" ) ; guiPtPub = node . newPublisher ( "gui//Point" , "lab5_msgs//GUIPointMsg" ) ; ParameterTree paramTree = node . newParameterTree ( ) ; mapFileName = paramTree . getString ( node . resolveName ( "~//mapFileName" ) ) ; try { guiErasePub . publish ( new GUIEraseMsg ( ) ) ; challengeMap = GrandChallengeMap . parseFile ( mapFileName ) ; obsCSpaces = cSpace . generateCSpace ( challengeMap , false ) ; challengeMap . set3DCSpace ( obsCSpaces ) ; rrt = new RRT ( challengeMap ) ; displayMap ( ) ; } catch ( Exception e ) { System . err . println ( ( "Failed<seq2seq4repair_space>trying<seq2seq4repair_space>to<seq2seq4repair_space>load<seq2seq4repair_space>file<seq2seq4repair_space>" + ( mapFileName ) ) ) ; e . printStackTrace ( ) ; } try { rrtPath = generateTestPath ( challengeMap . getRobotStart ( ) ) ; try { System . out . println ( "Starting<seq2seq4repair_space>up<seq2seq4repair_space>particle<seq2seq4repair_space>filter" ) ; particleFilter = new ParticleFilter ( 10000 , challengeMap , 0.0 , 0.0 , 0.0 ) ; publishParticles ( ) ; prevPt = robotStart ; for ( Point2D . Double pt : rrtPath ) { double transDist = RRT . getDist ( prevPt . x , prevPt . y , pt . x , pt . y ) ; double rotAng = RRT . getAngle ( prevPt . x , prevPt . y , pt . x , pt . y ) ; if ( rotAng < 0.0 ) { rotAng += 2 * ( Math . PI ) ; } particleFilter . motionUpdate ( transDist , rotAng ) ; ArrayList < Integer > measuredFiducials = getFidsInFOV ( pt ) ; HashMap < Integer , Double > measuredDists = getFidsDists ( pt , measuredFiducials ) ; particleFilter . measurementUpdate ( measuredFiducials , measuredDists ) ; prevPt = pt ; refreshDisplay ( ) ; <START_BUG> System . out . println ( particleFilter . toString ( ) ) ; <END_BUG> Thread . sleep ( 5000 ) ; } System . out . println ( "Done<seq2seq4repair_space>with<seq2seq4repair_space>particle<seq2seq4repair_space>filter" ) ; } catch ( Exception e ) { System . err . println ( "Failed<seq2seq4repair_space>to<seq2seq4repair_space>localize" ) ; e . printStackTrace ( ) ; } } catch ( Exception e ) { System . err . println ( "Failed<seq2seq4repair_space>to<seq2seq4repair_space>find<seq2seq4repair_space>path..." ) ; e . printStackTrace ( ) ; } } private ArrayList < Point2D . Double > generateTestPath ( Point2D . Double startPoint ) { } private ArrayList < Integer > getFidsInFOV ( Point2D . Double robotPos ) { } private HashMap < Integer , Double > getFidsDists ( Point2D . Double robotPos , ArrayList < Integer > measuredFiducials ) { } private void publishParticles ( ) { } private void refreshDisplay ( ) { } private void outputPath ( List < Point2D . Double > points , Color color ) { } private void displayMap ( ) { } private void displayMapCSpace ( ) { } protected void publishEllipse ( double x , double y , double w , double h , Color color ) { } protected void publishPoint ( double x , double y , int shape , Color color ) { } @ Override public void onShutdown ( Node node ) { } @ Override public void onShutdownComplete ( Node node ) { } @ Override public GraphName getDefaultNodeName ( ) { } No newline ; at end ; of file ; }

Fix:
particleFilter . printParticles ( ) ;