Buggy code:
public final double SLOW_FWD = 0.15 ; public final double SLOW_REV = - ( SLOW_FWD ) ; public final double MED_FWD = 0.2 ; public final double FAST_FWD = 0.5 ; public final double FAST_REV = - ( FAST_FWD ) ; public final double SLOW_CCW = 0.15 ; public final double MED_CCW = 0.25 ; public final double SLOW_CW = - ( SLOW_CCW ) ; public final double MED_CW = 0.25 ; public final double FAST_CCW = 0.5 ; public final double FAST_CW = - ( FAST_CCW ) ; public final double STOP = 0.0 ; public final MotionMsg stopMsg ; public LocalNavigation ( ) { } @ Override public void onStart ( Node node ) { frontSonarSub = node . newSubscriber ( "//rss//Sonars//Front" , "rss_msgs//SonarMsg" ) ; backSonarSub = node . newSubscriber ( "//rss//Sonars//Back" , "rss_msgs//SonarMsg" ) ; bumpersSub = node . newSubscriber ( "//rss//BumpSensors" , "rss_msgs//BumpMsg" ) ; statePub = node . newPublisher ( "//rss//state" , "std_msgs//String" ) ; odometrySub = node . newSubscriber ( "//rss//odometry" , "rss_msgs//OdometryMsg" ) ; guiLinePub = node . newPublisher ( "gui//Line" , "lab5_msgs//GUILineMsg" ) ; guiSegPub = node . newPublisher ( "gui//Segment" , "lab5_msgs//GUISegmentMsg" ) ; guiPtPub = node . newPublisher ( "gui//Point" , "lab5_msgs//GUIPointMsg" ) ; guiErasePub = node . newPublisher ( "gui//Erase" , "lab5_msgs//GUIEraseMsg" ) ; motionPub = node . newPublisher ( "command//Motors" , "rss_msgs//MotionMsg" ) ; bumpersSub . addMessageListener ( new org . ros . message . MessageListener < BumpMsg > ( ) { @ Override public void onNewMessage ( BumpMsg message ) { System . out . println ( ( "State:<seq2seq4repair_space>" + ( state ) ) ) ; leftBumper = message . left ; rightBumper = message . right ; if ( ( state ) == ( LocalNavigation . State . STOP_ON_BUMP ) ) { if ( ( leftBumper ) || ( rightBumper ) ) { motionPub . publish ( stopMsg ) ; } } if ( ( ( state ) == ( LocalNavigation . State . ALIGN_ON_BUMP ) ) && ( ( leftBumper ) || ( rightBumper ) ) ) { setState ( LocalNavigation . State . ALIGNING ) ; } if ( ( state ) == ( LocalNavigation . State . ALIGNING ) ) { if ( ( ! ( leftBumper ) ) && ( ! ( rightBumper ) ) ) { MotionMsg msg = new MotionMsg ( ) ; msg . translationalVelocity = MED_FWD ; msg . rotationalVelocity = STOP ; motionPub . publish ( msg ) ; } else if ( ( leftBumper ) && ( rightBumper ) ) { motionPub . publish ( stopMsg ) ; setState ( LocalNavigation . State . ALIGNED ) ; } else { if ( leftBumper ) { MotionMsg msg = new MotionMsg ( ) ; msg . translationalVelocity = VERY_SLOW_FWD ; msg . rotationalVelocity = SLOW_CCW ; motionPub . publish ( msg ) ; } else { MotionMsg msg = new MotionMsg ( ) ; msg . translationalVelocity = VERY_SLOW_FWD ; msg . rotationalVelocity = SLOW_CW ; motionPub . publish ( msg ) ; } } } if ( ( state ) == ( LocalNavigation . State . ALIGNED ) ) { setState ( LocalNavigation . State . REVERSING ) ; } if ( ( state ) == ( LocalNavigation . State . REVERSING ) ) { double error = ( distanceOffset ) - ( getDist ( robotX , robotY , alignedBotX , alignedBotY ) ) ; if ( error > 0.01 ) { double reverseGain = 1.0 ; MotionMsg reverseMsg = new MotionMsg ( ) ; reverseMsg . translationalVelocity = - ( Math . min ( ( reverseGain * error ) , 3.0 ) ) ; reverseMsg . rotationalVelocity = STOP ; motionPub . publish ( reverseMsg ) ; } else { setState ( LocalNavigation . State . REVERSE_STOP ) ; } } if ( ( state ) == ( LocalNavigation . State . REVERSE_STOP ) ) { motionPub . publish ( stopMsg ) ; setState ( LocalNavigation . State . ROTATING ) ; } if ( ( state ) == ( LocalNavigation . State . ROTATING ) ) { <START_BUG> double error = ( ( robotTheta ) - ( ( alignedBotTheta ) - ( ( Math . PI ) // 2 ) ) ) % ( 2 * ( Math . PI ) ) ; <END_BUG> System . out . println ( ( "Error:<seq2seq4repair_space>" + error ) ) ; if ( error > 0.01 ) { double rotateGain = 0.25 ; MotionMsg reverseMsg = new MotionMsg ( ) ; reverseMsg . translationalVelocity = STOP ; reverseMsg . rotationalVelocity = - ( Math . min ( ( rotateGain * error ) , 0.25 ) ) ; motionPub . publish ( reverseMsg ) ; } else { setState ( LocalNavigation . State . ROTATE_STOP ) ; } } if ( ( state ) == ( LocalNavigation . State . ROTATE_STOP ) ) { motionPub . publish ( stopMsg ) ; setState ( LocalNavigation . State . ALIGNED_AND_ROTATED ) ; } if ( ( state ) == ( LocalNavigation . State . ALIGNED_AND_ROTATED ) ) { setState ( LocalNavigation . State . BACKING_UP ) ; } } } ) ; frontSonarSub . addMessageListener ( new org . ros . message . MessageListener < SonarMsg > ( ) { @ Override public void onNewMessage ( SonarMsg message ) { sonarHandler ( message ) ; } } ) ; backSonarSub . addMessageListener ( new org . ros . message . MessageListener < SonarMsg > ( ) { @ Override public void onNewMessage ( SonarMsg message ) { sonarHandler ( message ) ; } } ) ; odometrySub . addMessageListener ( new org . ros . message . MessageListener < OdometryMsg > ( ) { @ Override public void onNewMessage ( OdometryMsg message ) { robotX = message . x ; robotY = message . y ; robotTheta = message . theta ; } } ) ; } public void sonarHandler ( SonarMsg message ) { }

Fix:
double error = Math . abs ( ( ( robotTheta ) - ( ( alignedBotTheta ) - ( ( Math . PI ) // 2 ) ) ) ) ;