diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/baseNodes/SerialNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/baseNodes/SerialNode.java index b55c93cf..74beaf87 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/baseNodes/SerialNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/baseNodes/SerialNode.java @@ -53,7 +53,7 @@ public SerialNode(BuggyNode base, String threadName, String portName, this.threadName = threadName; // need to accept a vaild port name - if (portName.equals("")) { + if (portName == null || portName.equals("")) { return; } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/RBSMNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/RBSMNode.java index 90fcf6a5..a6d42008 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/RBSMNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/RBSMNode.java @@ -93,6 +93,46 @@ public class RBSMNode extends SerialNode { private Publisher messagePubFp; //Fingerprint for low level hash + /** + * Error codes from the low level system + * Original definitions are in the rbsm_config.txt + */ + protected enum RBSMErrorCodes { + UNKNOWN_CODE(0, "Unknown RBSM Error!"), + WATCHDOG_TIMER(1, "Watchdog Timer fired"), + RBSM_LOST_STREAM(2, "RBSM Lost Stream"), + RBSM_INVALID_MID(4, "RBSM got an invalid Message ID!"), + RBSM_RC_LOST_SIGNAL(8, "RC Controller disconnected!"), + RBSM_AUTON_LOST_SIGNAL(16, "Auton signal lost!"), + ; + + private int errorCode; + private String errorMessage; + + RBSMErrorCodes(int errorCode, String errorMsg) { + this.errorCode = errorCode; + this.errorMessage = errorMsg; + } + + public static RBSMErrorCodes getErrorCodeForDataWord(int encoderDataWord) { + for (RBSMErrorCodes code : RBSMErrorCodes.values()) { + if (code.errorCode == encoderDataWord) { + return code; + } + } + return UNKNOWN_CODE; + } + + protected int getErrorCode() { + return errorCode; + } + + protected String getErrorMessage() { + return errorMessage; + } + + } + /** * Construct a new RBSMNode object @@ -216,9 +256,10 @@ public int peel(byte[] buffer, int start, int bytesAvailable) { messagePubEnc.publish(estimateVelocity(message.getDataWord())); } else if (headerNumber == RBSerialMessage.getHeaderByte("RBSM_MID_ERROR")) { // don't want to publish the status ok messages - // TODO change this to an enum if (message.getDataWord() != 0) { - new RobobuggyLogicNotification("RBSM_MID_ERROR:" + message.getDataWord(), RobobuggyMessageLevel.EXCEPTION); + RBSMErrorCodes errorCode = RBSMErrorCodes.getErrorCodeForDataWord(message.getDataWord()); + new RobobuggyLogicNotification("RBSM_MID_ERROR:" + errorCode.getErrorCode() + ": " + errorCode.getErrorMessage(), + RobobuggyMessageLevel.EXCEPTION); } } else if (headerNumber == RBSerialMessage.getHeaderByte("RBSM_MID_ENC_RESET_CONFIRM")) { new RobobuggyLogicNotification("Encoder Reset Confirmed by Zoe", RobobuggyMessageLevel.NOTE); diff --git a/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/RBSMNodeTest.java b/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/RBSMNodeTest.java new file mode 100644 index 00000000..4b5b576c --- /dev/null +++ b/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/RBSMNodeTest.java @@ -0,0 +1,101 @@ +package com.roboclub.robobuggy.nodes.sensors; + +import com.roboclub.robobuggy.messages.RobobuggyLogicNotificationMeasurement; +import com.roboclub.robobuggy.ros.Message; +import com.roboclub.robobuggy.ros.NodeChannel; +import com.roboclub.robobuggy.ros.Subscriber; +import org.junit.Before; +import org.junit.BeforeClass; +import org.junit.Test; + +import java.util.concurrent.LinkedBlockingQueue; + +import static org.junit.Assert.assertEquals; +import static org.junit.Assert.assertNotNull; +import static org.junit.Assert.fail; + +/** + * Created by vivaanbahl on 2/15/17. + */ +public class RBSMNodeTest { + private static LinkedBlockingQueue messages; + private RBSMNode testNode; + + private static final byte RBSM_MESSAGE_FOOTER = 0xA; + + /** + * Runs setup for the test class + */ + @BeforeClass + public static void oneTimeSetup() { + messages = new LinkedBlockingQueue<>(); + new Subscriber("RBSM Node Unit Tests", NodeChannel.LOGIC_NOTIFICATION.getMsgPath(), (topicName, m) -> { + messages.add(m); + }); + } + + /** + * Runs setup for each test case + */ + @Before + public void setUp() { + // get a dummy RBSMNode each time + testNode = new RBSMNode(NodeChannel.STEERING, NodeChannel.STEERING, null, 100); + } + + /** + * Tests the RBSM MID error translation from the bit representation to the + * error message + * + * Iterates through each known low-level error and calls peel() with a faked message + * + * Compares the output of peel() with the expected RBSMErrorCodes message format + * + * No errors expected + */ + @Test + public void testRbsmMidErrorTranslation() { + byte rbsmMidErrorHeader = (byte) 254; + byte[] fakeBuffer = { rbsmMidErrorHeader, 0, 0, 0, 0, RBSM_MESSAGE_FOOTER }; + + // test all ok, expect no response + testNode.peel(fakeBuffer, 0, 6); + // wait for the peel method to propagate + waitForMessagePropagation(); + // system ok shouldn't generate anything + assertEquals(messages.size(), 0); + + // test other error ids + // loop over the other values + // NOTE we skip the first one since it's a special case + for (int i = 1; i < RBSMNode.RBSMErrorCodes.values().length; i++) { + // get the ith error code, and put it in the buffer + RBSMNode.RBSMErrorCodes errorCode = RBSMNode.RBSMErrorCodes.values()[i]; + fakeBuffer[4] = (byte) errorCode.getErrorCode(); + testNode.peel(fakeBuffer, 0, 6); + + waitForMessagePropagation(); + + // make sure that we received a message + Message top = messages.poll(); + assertNotNull(top); + assertEquals(((RobobuggyLogicNotificationMeasurement) top).getMessage(), + "RBSM_MID_ERROR:" + errorCode.getErrorCode() + ": " + errorCode.getErrorMessage()); + } + + } + + /** + * Unfortunately we can't trigger continuation very easily, so we instead just wait + * for 100 ms for peel() to finish + */ + private void waitForMessagePropagation() { + try { + Thread.sleep(100); + } catch (InterruptedException e) { + e.printStackTrace(); + fail("Thread was interrupted"); + } + } + +} \ No newline at end of file