Merge pull request #456 from CMU-Robotics-Club/vivaan_rbsmErrorMsgs

added RBSM error codes as printouts rather than raw data words

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;
         }
 

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);

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<Message> 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");
+        }
+    }
+
+}