diff --git a/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ClientStateMachine.java b/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ClientStateMachine.java
index 1b416b4c1b8f9599ac23ec1a802cf7566bb496e7..6e975a3cdf298f37cffc9680d5226c4eee1730ce 100644
--- a/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ClientStateMachine.java
+++ b/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ClientStateMachine.java
@@ -89,7 +89,7 @@ public class ClientStateMachine {
     }
   }
 
-  public int latestGoalStatus;
+  int latestGoalStatus;
   int state;
   int nextState;
   private Log log = LogFactory.getLog(ActionClient.class);
@@ -439,17 +439,14 @@ public class ClientStateMachine {
    * @return True if the goal can be cancelled, false otherwise.
    */
   public boolean cancel() {
-    boolean ret;
     ArrayList<Integer> cancellableStates = new ArrayList<>(Arrays.asList(ClientStates.WAITING_FOR_GOAL_ACK,
         ClientStates.PENDING, ClientStates.ACTIVE));
-        
-        if (cancellableStates.contains(state)) {
-          state = ClientStates.WAITING_FOR_CANCEL_ACK;
-          ret = true;
-        } else {
-          ret = false;
-        }
-    return ret;
+    boolean shouldCancel = cancellableStates.contains(state);
+
+    if (shouldCancel) {
+      state = ClientStates.WAITING_FOR_CANCEL_ACK;
+    }
+    return shouldCancel;
   }
 
   public void resultReceived() {
@@ -462,4 +459,8 @@ public class ClientStateMachine {
   public void markAsLost()
   {
   }
+
+  public int getLatestGoalStatus() {
+    return latestGoalStatus;
+  }
 }
diff --git a/src/rosjava_actionlib/rosjava_actionlib/src/test/java/TestClientStateMachine.java b/src/rosjava_actionlib/rosjava_actionlib/src/test/java/TestClientStateMachine.java
index 82cb5a0d4d23f4b4fa137d54581d03dc423d992e..6b67d4caae4cc6e600b478ee7678e60c610f0085 100644
--- a/src/rosjava_actionlib/rosjava_actionlib/src/test/java/TestClientStateMachine.java
+++ b/src/rosjava_actionlib/rosjava_actionlib/src/test/java/TestClientStateMachine.java
@@ -7,120 +7,87 @@ import java.util.Arrays;
 import actionlib_msgs.GoalStatus;
 
 public class TestClientStateMachine {
-    private ClientStateMachine subject;
-   
-   // Executes before each test. 
+  private ClientStateMachine subject;
+
+  // Executes before each test.
   @Before
   public void setUp() {
     subject = new ClientStateMachine();
   }
-  
+
   @Test
   public void testGetState() {
-      // Setup
-      int expectedState = ClientStateMachine.ClientStates.WAITING_FOR_GOAL_ACK;
-      int actualState;
-      
-      subject.setState(expectedState);
-      
-      // Optional: Precondition
-      
-      // Exercise
-      actualState = subject.getState();
-      
-      // Assertion - Postcondition
-      assertEquals(expectedState, actualState);
+    int expectedState = ClientStateMachine.ClientStates.WAITING_FOR_GOAL_ACK;
+    int actualState;
+    subject.setState(expectedState);
+    actualState = subject.getState();
+    assertEquals(expectedState, actualState);
   }
-  
+
   @Test
   public void testSetState() {
-       // Setup
-      int expectedState = ClientStateMachine.ClientStates.WAITING_FOR_GOAL_ACK;
-     
-      // Optional: Precondition 
-      assertEquals(subject.getState(), 0);
-      
-      // Exercise
-      subject.setState(expectedState);
-      
-      // Assertion - Postcondition
-      assertEquals(expectedState, subject.getState());
+    int expectedState = ClientStateMachine.ClientStates.WAITING_FOR_GOAL_ACK;
+    assertEquals(subject.getState(), 0);
+    subject.setState(expectedState);
+    assertEquals(expectedState, subject.getState());
   }
-  
+
   @Test
   public void testUpdateStatusWhenStateIsNotDone() {
-       // Setup
-      int expectedStatus = 7;
-      subject.setState(ClientStateMachine.ClientStates.WAITING_FOR_GOAL_ACK);
-     
-      // Optional: Precondition 
-      assertEquals(0, subject.latestGoalStatus);
-      
-      // Exercise
-      subject.updateStatus(expectedStatus);
-      
-      // Assertion - Postcondition
-      assertEquals(expectedStatus, subject.latestGoalStatus);
+    int expectedStatus = 7;
+    subject.setState(ClientStateMachine.ClientStates.WAITING_FOR_GOAL_ACK);
+    assertEquals(0, subject.getLatestGoalStatus());
+    subject.updateStatus(expectedStatus);
+    assertEquals(expectedStatus, subject.getLatestGoalStatus());
   }
-  
+
   @Test
   public void testUpdateStatusWhenStateIsDone() {
-       // Setup
-      subject.setState(ClientStateMachine.ClientStates.DONE);
-     
-      // Optional: Precondition 
-      assertEquals(0, subject.latestGoalStatus);
-      
-      // Exercise
-      subject.updateStatus(7);
-      
-      // Assertion - Postcondition
-      assertEquals(0, subject.latestGoalStatus);
+    subject.setState(ClientStateMachine.ClientStates.DONE);
+    assertEquals(0, subject.getLatestGoalStatus());
+    subject.updateStatus(7);
+    assertEquals(0, subject.getLatestGoalStatus());
   }
-  
+
   @Test
   public void testCancelOnCancellableStates() {
-      checkCancelOnInitialCancellableState(ClientStateMachine.ClientStates.WAITING_FOR_GOAL_ACK);
-      checkCancelOnInitialCancellableState(ClientStateMachine.ClientStates.PENDING);
-      checkCancelOnInitialCancellableState(ClientStateMachine.ClientStates.ACTIVE);
+    checkCancelOnInitialCancellableState(ClientStateMachine.ClientStates.WAITING_FOR_GOAL_ACK);
+    checkCancelOnInitialCancellableState(ClientStateMachine.ClientStates.PENDING);
+    checkCancelOnInitialCancellableState(ClientStateMachine.ClientStates.ACTIVE);
   }
-  
+
   @Test
   public void testCancelOnNonCancellableStates() {
-      checkCancelOnInitialNonCancellableState(ClientStateMachine.ClientStates.INVALID_TRANSITION);
-      checkCancelOnInitialNonCancellableState(ClientStateMachine.ClientStates.NO_TRANSITION);
-      checkCancelOnInitialNonCancellableState(ClientStateMachine.ClientStates.WAITING_FOR_RESULT);
-      checkCancelOnInitialNonCancellableState(ClientStateMachine.ClientStates.WAITING_FOR_CANCEL_ACK);
-      checkCancelOnInitialNonCancellableState(ClientStateMachine.ClientStates.RECALLING);
-      checkCancelOnInitialNonCancellableState(ClientStateMachine.ClientStates.PREEMPTING);
-      checkCancelOnInitialNonCancellableState(ClientStateMachine.ClientStates.DONE);
-      checkCancelOnInitialNonCancellableState(ClientStateMachine.ClientStates.LOST);
-  }  
-  
+    checkCancelOnInitialNonCancellableState(ClientStateMachine.ClientStates.INVALID_TRANSITION);
+    checkCancelOnInitialNonCancellableState(ClientStateMachine.ClientStates.NO_TRANSITION);
+    checkCancelOnInitialNonCancellableState(ClientStateMachine.ClientStates.WAITING_FOR_RESULT);
+    checkCancelOnInitialNonCancellableState(ClientStateMachine.ClientStates.WAITING_FOR_CANCEL_ACK);
+    checkCancelOnInitialNonCancellableState(ClientStateMachine.ClientStates.RECALLING);
+    checkCancelOnInitialNonCancellableState(ClientStateMachine.ClientStates.PREEMPTING);
+    checkCancelOnInitialNonCancellableState(ClientStateMachine.ClientStates.DONE);
+    checkCancelOnInitialNonCancellableState(ClientStateMachine.ClientStates.LOST);
+  }
+
   private void checkCancelOnInitialCancellableState(int initialState) {
     subject.setState(initialState);
-      
-      assertTrue("Failed test on initial state " + initialState, subject.cancel());
-      
-      assertEquals("Failed test on initial state " + initialState, ClientStateMachine.ClientStates.WAITING_FOR_CANCEL_ACK, subject.getState());
+    assertTrue("Failed test on initial state " + initialState, subject.cancel());
+    assertEquals("Failed test on initial state " + initialState, ClientStateMachine.ClientStates.WAITING_FOR_CANCEL_ACK, subject.getState());
   }
 
 
   private void checkCancelOnInitialNonCancellableState(int initialState) {
     subject.setState(initialState);
-      
-      assertFalse("Failed test on initial state " + initialState, subject.cancel());
-      
-      assertEquals("Failed test on initial state " + initialState, initialState, subject.getState());
+    assertFalse("Failed test on initial state " + initialState, subject.cancel());
+    assertEquals("Failed test on initial state " + initialState, initialState, subject.getState());
   }
-  
+
   @Test
   public void testResultReceivedWhileWaitingForResult() {
     subject.setState(ClientStateMachine.ClientStates.WAITING_FOR_RESULT);
     subject.resultReceived();
     assertEquals(ClientStateMachine.ClientStates.DONE, subject.getState());
   }
-  
+
   @Test
   public void testResultReceivedWhileNotWaitingForResult() {
     checkResultReceivedWhileNotWaitingForResult(ClientStateMachine.ClientStates.INVALID_TRANSITION);
@@ -134,26 +101,26 @@ public class TestClientStateMachine {
     checkResultReceivedWhileNotWaitingForResult(ClientStateMachine.ClientStates.DONE);
     checkResultReceivedWhileNotWaitingForResult(ClientStateMachine.ClientStates.LOST);
   }
-  
+
   private void checkResultReceivedWhileNotWaitingForResult(int state) {
     subject.setState(state);
     subject.resultReceived();
-    assertEquals("Failed test on initial state " + state, state, subject.getState());    
+    assertEquals("Failed test on initial state " + state, state, subject.getState());
   }
-  
+
   @Test
   public void testGetTrasition() {
-      Vector<Integer> expected;
-      expected = new Vector<>(Arrays.asList(ClientStateMachine.ClientStates.PENDING));
-      checkGetTransition(ClientStateMachine.ClientStates.WAITING_FOR_GOAL_ACK,
-        actionlib_msgs.GoalStatus.PENDING, expected);
-        
-      expected = new Vector<>(Arrays.asList(ClientStateMachine.ClientStates.PENDING,
-        ClientStateMachine.ClientStates.WAITING_FOR_RESULT));
-      checkGetTransition(ClientStateMachine.ClientStates.WAITING_FOR_GOAL_ACK,
-        actionlib_msgs.GoalStatus.REJECTED, expected);
+    Vector<Integer> expected;
+    expected = new Vector<>(Arrays.asList(ClientStateMachine.ClientStates.PENDING));
+    checkGetTransition(ClientStateMachine.ClientStates.WAITING_FOR_GOAL_ACK,
+      actionlib_msgs.GoalStatus.PENDING, expected);
+
+    expected = new Vector<>(Arrays.asList(ClientStateMachine.ClientStates.PENDING,
+      ClientStateMachine.ClientStates.WAITING_FOR_RESULT));
+    checkGetTransition(ClientStateMachine.ClientStates.WAITING_FOR_GOAL_ACK,
+      actionlib_msgs.GoalStatus.REJECTED, expected);
   }
-  
+
   private void checkGetTransition(int initialState, int goalStatus, Vector<Integer> expected) {
     subject.setState(initialState);
     Vector<Integer> output = subject.getTransition(goalStatus);