Skip to content
Snippets Groups Projects
Commit 2adbbf98 authored by Ernesto Corbellini's avatar Ernesto Corbellini
Browse files

Added more unit tests and cleanup.

parent 1027eb0c
No related branches found
No related tags found
No related merge requests found
......@@ -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;
......@@ -87,7 +89,7 @@ public class ClientStateMachine {
}
}
int latestGoalStatus;
public int latestGoalStatus;
int state;
int nextState;
private Log log = LogFactory.getLog(ActionClient.class);
......@@ -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,14 +439,15 @@ 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:
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;
break;
} else {
ret = false;
}
return ret;
}
......@@ -471,6 +458,7 @@ public class ClientStateMachine {
}
}
// TODO: implement method
public void markAsLost()
{
}
......
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() {
// 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);
}
@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());
}
@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);
}
@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);
}
@Test
public void testCancelOnCancellableStates() {
checkCancelOnInitialCancellableState(ClientStateMachine.ClientStates.WAITING_FOR_GOAL_ACK);
checkCancelOnInitialCancellableState(ClientStateMachine.ClientStates.PENDING);
checkCancelOnInitialCancellableState(ClientStateMachine.ClientStates.ACTIVE);
}
@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 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);
}
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment