Buggy code:
private boolean obsDetectBack = false ; private double frontSonarDist = 0.0 ; private double backSonarDist = 0.0 ; private ColorMsg redMsg ; private ColorMsg greenMsg ; private ColorMsg blueMsg ; private ColorMsg blackMsg ; public LineEstimator lineEstimator = new LineEstimator ( ) ; public enum State { STOP_ON_BUMP , ALIGN_ON_BUMP , ALIGNING , ALIGNED , REVERSING , REVERSE_STOP , ROTATING , ROTATE_STOP , ALIGNED_AND_ROTATED , BACKING_UP , FINDING_WALL , TRACKING_WALL , WALL_ENDED , DONE ; } public final double VERY_SLOW_FWD = 0.1 ; 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 ) { } public void sonarHandler ( SonarMsg message ) { if ( ( message . range ) < ( threshold ) ) { if ( message . isFront ) { frontSonarDist = message . range ; obsDetectFront = true ; } else { backSonarDist = message . range ; obsDetectBack = true ; } } else { if ( message . isFront ) { obsDetectFront = false ; } else { obsDetectBack = false ; } } GUIPointMsg ptMsg = new GUIPointMsg ( ) ; if ( message . isFront ) { ptMsg . x = ( ( robotX ) + ( ( Math . cos ( robotTheta ) ) * ( FRONT_SONAR_Y ) ) ) - ( ( Math . sin ( robotTheta ) ) * ( ( message . range ) - ( FRONT_SONAR_X ) ) ) ; ptMsg . y = ( ( robotY ) + ( ( Math . sin ( robotTheta ) ) * ( FRONT_SONAR_Y ) ) ) + ( ( Math . cos ( robotTheta ) ) * ( ( message . range ) - ( FRONT_SONAR_X ) ) ) ; ptMsg . color = redMsg ; } else { ptMsg . x = ( ( robotX ) + ( ( Math . cos ( robotTheta ) ) * ( BACK_SONAR_Y ) ) ) - ( ( Math . sin ( robotTheta ) ) * ( ( message . range ) - ( BACK_SONAR_X ) ) ) ; ptMsg . y = ( ( robotY ) + ( ( Math . sin ( robotTheta ) ) * ( BACK_SONAR_Y ) ) ) + ( ( Math . cos ( robotTheta ) ) * ( ( message . range ) - ( BACK_SONAR_X ) ) ) ; ptMsg . color = blueMsg ; } if ( ! ( ( obsDetectFront ) || ( obsDetectBack ) ) ) { ptMsg . color = greenMsg ; } else { ptMsg . color = redMsg ; } guiPtPub . publish ( ptMsg ) ; lineEstimator . updateTerms ( ptMsg . x , ptMsg . y ) ; GUILineMsg lineMsg = new GUILineMsg ( ) ; lineMsg . lineA = lineEstimator . getA ( ) ; lineMsg . lineB = lineEstimator . getB ( ) ; lineMsg . lineC = lineEstimator . getC ( ) ; lineMsg . color = redMsg ; guiLinePub . publish ( lineMsg ) ; if ( ( state ) == ( LocalNavigation . State . BACKING_UP ) ) { if ( ( obsDetectFront ) || ( obsDetectBack ) ) { MotionMsg msg = new MotionMsg ( ) ; msg . translationalVelocity = SLOW_REV ; msg . rotationalVelocity = STOP ; motionPub . publish ( msg ) ; } else { motionPub . publish ( stopMsg ) ; setState ( LocalNavigation . State . FINDING_WALL ) ; } } if ( ( state ) == ( LocalNavigation . State . FINDING_WALL ) ) { if ( ! ( ( obsDetectFront ) || ( obsDetectBack ) ) ) { MotionMsg msg = new MotionMsg ( ) ; msg . translationalVelocity = SLOW_FWD ; msg . rotationalVelocity = STOP ; motionPub . publish ( msg ) ; } else { setState ( LocalNavigation . State . TRACKING_WALL ) ; lineEstimator . resetFilter ( ) ; } } if ( ( state ) == ( LocalNavigation . State . TRACKING_WALL ) ) { if ( ! ( ( obsDetectFront ) || ( obsDetectBack ) ) ) { motionPub . publish ( stopMsg ) ; setState ( LocalNavigation . State . WALL_ENDED ) ; } else { double transGain = 0.125 ; double rotGain = 0.25 ; double transError = ( lineEstimator . getPerpDist ( robotX , robotY ) ) - ( distanceOffset ) ; double obsSlope = Math . atan2 ( ( - ( lineEstimator . getA ( ) ) ) , lineEstimator . getB ( ) ) ; double orientError = obsSlope - ( robotTheta ) ; MotionMsg msg = new MotionMsg ( ) ; msg . translationalVelocity = SLOW_FWD ; <START_BUG> msg . rotationalVelocity = ( ( trackingGain ) * transError ) + ( rotGain * orientError ) ; <END_BUG> motionPub . publish ( msg ) ; } } if ( ( state ) == ( LocalNavigation . State . WALL_ENDED ) ) { setState ( LocalNavigation . State . ALIGN_ON_BUMP ) ; } } public void setState ( LocalNavigation . State newState ) { } public double getDist ( double pt1X , double pt1Y , double pt2X , double pt2Y ) { } private void generateColorMsgs ( ) { } @ Override public void onShutdown ( Node node ) { } @ Override public void onShutdownComplete ( Node node ) { } @ Override public GraphName getDefaultNodeName ( ) { } }

Fix:
msg . rotationalVelocity = ( transGain * transError ) + ( rotGain * orientError ) ;