diff --git a/README.md b/README.md
index 23a88edb47f9000cb3e5978dcc9837a15916ee42..3eddbd0876b0532cbe5058369b49332c3643bc73 100644
--- a/README.md
+++ b/README.md
@@ -92,3 +92,10 @@ roslaunch rosjava_actionlib server_demo.launch --screen
 ```
 
 Use Ctrl+C to stop the execution once it's finished.
+
+
+## Running unit tests
+```
+$ cd src/rosjava_actionlib/
+$ ./gradlew test
+```
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 5fd7a2a390ec43fa1ddc0c53c725d45183c6d428..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
@@ -20,6 +20,8 @@ import java.lang.Exception;
 import actionlib_msgs.GoalStatus;
 import java.util.Vector;
 import java.util.Iterator;
+import java.util.ArrayList;
+import java.util.Arrays;
 import org.apache.commons.logging.Log;
 import org.apache.commons.logging.LogFactory;
 
@@ -108,23 +110,7 @@ public class ClientStateMachine {
   {
     if (this.state != ClientStates.DONE)
     {
-      /*status = this.goal.findStatus(statusArray); //_find_status_by_goal_id(statusArray, self.action_goal.goal_id.id);
-
-      // we haven't heard of the goal?
-      if (status == 0)
-      {
-        if (this.state != ClientStates.WAITING_FOR_GOAL_ACK && this.state != ClientStates.WAITING_FOR_RESULT && this.state != ClientStates.DONE)
-        {
-          markAsLost();
-        }
-        return;
-      }*/
-
       this.latestGoalStatus = status;
-
-      // Determine the next state
-
-      //if (this.state
     }
   }
 
@@ -453,16 +439,14 @@ public class ClientStateMachine {
    * @return True if the goal can be cancelled, false otherwise.
    */
   public boolean cancel() {
-    boolean ret = false;
-    switch (state) {
-      case ClientStates.WAITING_FOR_GOAL_ACK:
-      case ClientStates.PENDING:
-      case ClientStates.ACTIVE:
-        state = ClientStates.WAITING_FOR_CANCEL_ACK;
-        ret = true;
-        break;
+    ArrayList<Integer> cancellableStates = new ArrayList<>(Arrays.asList(ClientStates.WAITING_FOR_GOAL_ACK,
+        ClientStates.PENDING, ClientStates.ACTIVE));
+    boolean shouldCancel = cancellableStates.contains(state);
+
+    if (shouldCancel) {
+      state = ClientStates.WAITING_FOR_CANCEL_ACK;
     }
-    return ret;
+    return shouldCancel;
   }
 
   public void resultReceived() {
@@ -471,7 +455,12 @@ public class ClientStateMachine {
     }
   }
 
+  // TODO: implement method
   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 4675e3596a6d6f5f3f55d926ab4941a7133704c3..6b67d4caae4cc6e600b478ee7678e60c610f0085 100644
--- a/src/rosjava_actionlib/rosjava_actionlib/src/test/java/TestClientStateMachine.java
+++ b/src/rosjava_actionlib/rosjava_actionlib/src/test/java/TestClientStateMachine.java
@@ -1,37 +1,129 @@
 import com.github.ekumen.rosjava_actionlib.*;
-import static org.junit.Assert.assertEquals;
+import static org.junit.Assert.*;
 import org.junit.Test;
+import org.junit.Before;
 import java.util.Vector;
+import java.util.Arrays;
 import actionlib_msgs.GoalStatus;
 
 public class TestClientStateMachine {
+  private ClientStateMachine subject;
+
+  // Executes before each test.
+  @Before
+  public void setUp() {
+    subject = new ClientStateMachine();
+  }
+
+  @Test
+  public void testGetState() {
+    int expectedState = ClientStateMachine.ClientStates.WAITING_FOR_GOAL_ACK;
+    int actualState;
+    subject.setState(expectedState);
+    actualState = subject.getState();
+    assertEquals(expectedState, actualState);
+  }
+
   @Test
-  public void test1() {
-    // test a full branch transition from goal ack to waiting for result
-    int state;
-    ClientStateMachine stateMachine = new ClientStateMachine();
-
-    // set initial state
-    stateMachine.setState(ClientStateMachine.ClientStates.WAITING_FOR_GOAL_ACK);
-    // transition to next states
-    stateMachine.transition(actionlib_msgs.GoalStatus.ACTIVE);
-    assertEquals(ClientStateMachine.ClientStates.ACTIVE, stateMachine.getState());
-    stateMachine.transition(actionlib_msgs.GoalStatus.PREEMPTING);
-    assertEquals(ClientStateMachine.ClientStates.PREEMPTING, stateMachine.getState());
-    stateMachine.transition(actionlib_msgs.GoalStatus.SUCCEEDED);
-    assertEquals(ClientStateMachine.ClientStates.WAITING_FOR_RESULT, stateMachine.getState());
-  }
-
-  @Test
-  public void test2() {
-    // test a vector of states transition for a skipped test
-    int state;
-    ClientStateMachine stateMachine = new ClientStateMachine();
-
-    // set initial state
-    stateMachine.setState(ClientStateMachine.ClientStates.WAITING_FOR_CANCEL_ACK);
-    // transition to next states
-    stateMachine.transition(actionlib_msgs.GoalStatus.RECALLED);
-    assertEquals(ClientStateMachine.ClientStates.WAITING_FOR_RESULT, stateMachine.getState());
+  public void testSetState() {
+    int expectedState = ClientStateMachine.ClientStates.WAITING_FOR_GOAL_ACK;
+    assertEquals(subject.getState(), 0);
+    subject.setState(expectedState);
+    assertEquals(expectedState, subject.getState());
+  }
+
+  @Test
+  public void testUpdateStatusWhenStateIsNotDone() {
+    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() {
+    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);
+  }
+
+  @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);
+  }
+
+  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());
+  }
+
+
+  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());
+  }
+
+  @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);
+    checkResultReceivedWhileNotWaitingForResult(ClientStateMachine.ClientStates.NO_TRANSITION);
+    checkResultReceivedWhileNotWaitingForResult(ClientStateMachine.ClientStates.WAITING_FOR_GOAL_ACK);
+    checkResultReceivedWhileNotWaitingForResult(ClientStateMachine.ClientStates.PENDING);
+    checkResultReceivedWhileNotWaitingForResult(ClientStateMachine.ClientStates.ACTIVE);
+    checkResultReceivedWhileNotWaitingForResult(ClientStateMachine.ClientStates.WAITING_FOR_CANCEL_ACK);
+    checkResultReceivedWhileNotWaitingForResult(ClientStateMachine.ClientStates.RECALLING);
+    checkResultReceivedWhileNotWaitingForResult(ClientStateMachine.ClientStates.PREEMPTING);
+    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());
+  }
+
+  @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);
+  }
+
+  private void checkGetTransition(int initialState, int goalStatus, Vector<Integer> expected) {
+    subject.setState(initialState);
+    Vector<Integer> output = subject.getTransition(goalStatus);
+    assertEquals(expected, output);
   }
 }