parse realtime msg

pom.xml

@@ -210,6 +210,23 @@
 			<groupId>org.nanohttpd</groupId>
 			<version>2.3.2-SNAPSHOT</version>
 		</dependency>
+		<dependency>
+			<artifactId>nanohttpd-websocket</artifactId>
+			<groupId>org.nanohttpd</groupId>
+			<version>2.3.2-SNAPSHOT</version>
+		</dependency>
+
+		<dependency>
+			<artifactId>nanohttpd-webserver</artifactId>
+			<groupId>org.nanohttpd</groupId>
+			<version>2.3.2-SNAPSHOT</version>
+		</dependency>
+
+		<dependency>
+			<groupId>com.eclipsesource.minimal-json</groupId>
+			<artifactId>minimal-json</artifactId>
+			<version>0.9.5</version>
+		</dependency>
 
 		<!-- https://mvnrepository.com/artifact/org.junit.jupiter/junit-jupiter-api -->
 		<dependency>
@@ -218,6 +235,21 @@
 			<version>5.3.1</version>
 			<scope>test</scope>
 		</dependency>
+		<dependency>
+			<groupId>org.nanohttpd</groupId>
+			<artifactId>nanohttpd-nanolets</artifactId>
+			<version>2.3.2-SNAPSHOT</version>
+		</dependency>
+		<dependency>
+			<groupId>org.slf4j</groupId>
+			<artifactId>slf4j-api</artifactId>
+			<version>1.7.25</version>
+		</dependency>
+		<dependency>
+			<groupId>org.slf4j</groupId>
+			<artifactId>slf4j-jdk14</artifactId>
+			<version>1.7.25</version>
+		</dependency>
 
 
 	</dependencies>

src/main/java/at/acdp/urweb/Activator.java

@@ -16,7 +16,7 @@ public class Activator implements BundleActivator {
 		context.registerService(SwingProgramNodeService.class, new PickOrPlaceProgramNodeService(), null);
 		context.registerService(SwingProgramNodeService.class, new GripperOpenProgramNodeService(), null);
 		context.registerService(SwingProgramNodeService.class, new GripperCloseProgramNodeService(), null);
-		new WebServer().start();
+		new WebServer(8080, true).start();
 	}
 
 	@Override

src/main/java/at/acdp/urweb/Main.java

@@ -1,5 +1,7 @@
 package at.acdp.urweb;
 
+import at.acdp.urweb.web.GetRobotRealtimeData;
+import at.acdp.urweb.web.RTDE;
 import at.acdp.urweb.web.WebServer;
 
 import java.io.IOException;
@@ -9,7 +11,11 @@ public class Main {
     public static void main(String [ ] args) {
 
         try {
-            new WebServer().start();
+            new WebServer(8080,true).start();
+            //GetRobotRealtimeData rd=new GetRobotRealtimeData("192.168.20.107", 30003);
+            RTDE rde=new RTDE("192.168.20.107", 30002);
+            Thread t=new Thread(rde);
+            t.start();
 
         } catch (IOException e) {
             e.printStackTrace();

src/main/java/at/acdp/urweb/web/GetRobotRealtimeData.java

@@ -0,0 +1,191 @@
+package at.acdp.urweb.web;
+
+import java.io.DataInputStream;
+import java.io.IOException;
+import java.net.Socket;
+
+import java.net.SocketException;
+import java.util.HashMap;
+import java.util.Map;
+
+public class GetRobotRealtimeData implements Runnable {
+
+    private final String ip;
+    private final int port;
+    private volatile boolean _running=true;
+
+    /*****
+     * Creates a new RealTime reader
+     * Default Constructor
+     * Uses localhost (127.0.0.1) and port 30003
+     */
+    public GetRobotRealtimeData(String ip, int port) {
+        this.ip=ip;
+        this.port=port;
+
+    }
+
+    // Internal method that actually reads the data
+    private void readSocket() throws IOException {
+        try(Socket rt = new Socket(ip, port);){
+            rt.setSoTimeout(1000);
+
+            if (rt.isConnected()){
+                System.out.println("Connected to UR Realtime Client");
+            }
+
+            DataInputStream in = new DataInputStream(rt.getInputStream());
+            while(true) {
+                int length = in.readInt();
+                double[] rtm = new double[length];
+                rtm[0] = length;
+                // Calculate how much data is available from the length
+                int data_available = (length - 4) / 8;
+                for(int i=0; i<data_available; i++){
+                    rtm[i] = in.readDouble();
+                }
+            }
+        }
+
+    }
+
+    @Override
+    public void run() {
+        while(_running) {
+            try {
+                readSocket();
+            } catch (IOException e) {
+                e.printStackTrace();
+            }
+        }
+
+
+    }
+
+    /*
+     * Creating enum to map start index and lengths of data entries
+     * According to specification of RealTimeClient in Excel sheet
+     */
+    private enum RTinfo {
+        // name			(index in plot, number of doubles)
+        q_target		(2, 6),
+        qd_target		(8, 6),
+        qdd_target		(14, 6),
+        q_actual		(32, 6),
+        qd_actual		(38, 6),
+        TCP_actual		(56, 6),
+        TCPd_actual		(62, 6),
+        TCP_force		(68, 6),
+        TCP_target		(74, 6),
+        TCPd_target		(80, 6),
+        temp_joint		(87, 6),
+        robotmode		(95, 1),
+        jointmode		(96, 6),
+        safetymode		(97, 1),
+        tcp_accel		(109, 3),
+        speedscaling	(118, 1),
+        prgstate		(132, 1);
+        // More data points could be added if desired, by following above example and according to specification.
+
+        private final int index;
+        private final int count;
+        RTinfo(int index, int count){
+            this.index = index;
+            this.count = count;
+        }
+        private int index() {return index;}
+        private int count() {return count;}
+    }
+
+    /*****************************************************************
+     * Methods for returning the relevant data to the calling classes
+     *****************************************************************/
+
+    /*
+     * Example get function, to read actual joint positions as double[]
+     */
+    public double[] getActualJointPose(double []rtm){
+        double[] val = new double[RTinfo.q_actual.count()];
+        int i = 0;
+        while (i < RTinfo.q_actual.count()){
+            val[i] = rtm[RTinfo.q_actual.index()+i];
+            ++i;
+        }
+        return val;
+    }
+
+    /*
+     * Example get function, to read actual TCP position as double[]
+     */
+    public double[] getActualTcpPose(double[] rtm){
+        double[] val = new double[RTinfo.TCP_actual.count()];
+        int i = 0;
+        while (i < RTinfo.TCP_actual.count()){
+            val[i] = rtm[RTinfo.TCP_actual.index()+i];
+            ++i;
+        }
+        return val;
+    }
+
+    /*
+     * Example get function, to read joint temperatures as double[]
+     */
+    public double[] getJointTemperatures(double[] rtm){
+        double[] val = new double[RTinfo.temp_joint.count()];
+        int i = 0;
+        while (i < RTinfo.temp_joint.count()){
+            val[i] = rtm[RTinfo.temp_joint.index()+i];
+            ++i;
+        }
+        return val;
+    }
+
+    /*
+     * Example to read joint state as useful information
+     */
+    public String[] getJointStatus(double[] rtm){
+        // Create a map binding message code to state message
+        // According to Excel sheet client interface specification
+        Map<Double, String> jointStates = new HashMap<Double, String>();
+        jointStates.put((double) 236, "SHUTTING_DOWN");
+        jointStates.put((double) 237, "DUAL_CALIB_MODE");
+        jointStates.put((double) 238, "BACKDRIVE");
+        jointStates.put((double) 239, "POWER_OFF");
+        jointStates.put((double) 245, "NOT_RESPONDING");
+        jointStates.put((double) 246, "MOTOR_INIT");
+        jointStates.put((double) 247, "BOOTING");
+        jointStates.put((double) 248, "DUAL_CALIB_ERROR");
+        jointStates.put((double) 249, "BOOTLOADER");
+        jointStates.put((double) 250, "CALIBRATION_MODE");
+        jointStates.put((double) 252, "FAULT");
+        jointStates.put((double) 253, "RUNNING");
+        jointStates.put((double) 255, "IDLE");
+
+        String[] val = new String[RTinfo.jointmode.count()];
+        int i = 0;
+        while (i < RTinfo.jointmode.count()){
+            // Read the code for given joint
+            double code = rtm[RTinfo.jointmode.index()+i];
+
+            // Check if the key is known in the map
+            if(jointStates.containsKey(code)){
+                // Read corresponding message
+                val[i] = jointStates.get(code);
+            }
+            else{
+                // If unknown code, show "unknown"
+                val[i] = "UNKNOWN_CODE";
+            }
+            ++i;
+        }
+        return val;
+    }
+
+    /*
+     * Example to read the actual safety limited speed scaling in percent
+     */
+    public double getSpeedScaling(double[] rtm){
+        double scaling = rtm[RTinfo.speedscaling.index()];
+        return scaling * 100; // Converted to percent
+    }
+}

src/main/java/at/acdp/urweb/web/RTDE.java

@@ -0,0 +1,88 @@
+package at.acdp.urweb.web;
+import java.io.*;
+import java.net.Socket;
+import java.nio.charset.StandardCharsets;
+import java.util.HashMap;
+import java.util.Map;
+
+public class RTDE implements Runnable {
+
+    private final String ip;
+    private final int port;
+    private volatile boolean _running=true;
+    private Socket rt;
+    private OutputStream os;
+    private DataInputStream in;
+
+    public RTDE(String ip, int port) {
+        this.ip=ip;
+        this.port=port;
+    }
+    private void connect() throws IOException {
+        this.rt = new Socket(ip, port);
+        this.os=rt.getOutputStream();
+        this.in=new DataInputStream(rt.getInputStream());
+        readVersionMessage();
+    }
+
+    private void readVersionMessage() throws IOException {
+        int msgSize=in.readInt();
+        int msgType=in.readByte()&0xff;
+        long tstamp=in.readLong();
+        byte source=in.readByte();
+        byte robotMsgType=in.readByte();
+        byte projectNameSize=in.readByte();
+        byte[] name=new byte[projectNameSize];
+        in.read(name);
+        String nameS=new String(name);
+        int  majorVersion=in.readByte() & 0xff;
+        int  minorVersion=in.readByte() & 0xff;
+        int bugFixVersion=in.readInt();
+        int buildNumber=in.readInt();
+        byte[] buildDateB=new byte[msgSize-(16+projectNameSize+10)];
+        in.read(buildDateB);
+        String buildDate=new String(buildDateB);
+
+
+
+    }
+
+    private void writeCmd(String cmd) {
+        try {
+            System.out.println("send cmd:"+cmd);
+            os.write(cmd.getBytes(StandardCharsets.UTF_8));
+            int length=in.readInt();
+            System.out.println("len: "+length);
+            byte[] buf=new byte[length];
+            //in.read(buf);
+            int mType = in.readByte() & 0xff;
+            System.out.println("mtype: "+mType);
+            while(true) {
+                int sublength=in.readInt();
+                System.out.println("sublength: " + sublength);
+
+                int subType = in.readByte();
+                System.out.println("subtype: "+subType);
+                byte[] res=new byte[sublength-5];
+                in.read(res);
+
+
+            }
+        } catch (IOException e) {
+            e.printStackTrace();
+        }
+    }
+
+    @Override
+    public void run() {
+            try {
+                connect();
+                writeCmd("set_digital_out(1,True)\n");
+                writeCmd("set_digital_out(2,True)\n");
+                writeCmd("movej([-1.95,-1.58,-1.16,-1.15,-1.55,1.18], a=1.0, v=0.1)\n");
+
+            } catch (IOException e) {
+                e.printStackTrace();
+            }
+    }
+}

src/main/java/at/acdp/urweb/web/WebServer.java

@@ -1,83 +1,208 @@
 package at.acdp.urweb.web;
-import org.nanohttpd.protocols.http.IHTTPSession;
-import org.nanohttpd.protocols.http.NanoHTTPD;
-import org.nanohttpd.protocols.http.response.Response;
-import org.nanohttpd.util.ServerRunner;
-
 
-import java.util.HashMap;
-import java.util.List;
+import java.io.BufferedInputStream;
+import java.io.File;
+import java.io.IOException;
+import java.security.MessageDigest;
+import java.security.NoSuchAlgorithmException;
+import java.util.Base64;
 import java.util.Map;
+import java.util.Timer;
+import java.util.concurrent.atomic.AtomicLong;
+import java.util.logging.Logger;
 
-import static org.nanohttpd.protocols.http.response.Response.newFixedLengthResponse;
-
-public class WebServer extends NanoHTTPD {
-    public WebServer() {
-        super(8080);
+import com.eclipsesource.json.Json;
+import com.eclipsesource.json.JsonArray;
+import org.nanohttpd.protocols.http.IHTTPSession;
+import org.nanohttpd.protocols.http.response.IStatus;
+import org.nanohttpd.protocols.http.response.Response;
+import org.nanohttpd.protocols.http.response.Status;
+import org.nanohttpd.protocols.websockets.CloseCode;
+import org.nanohttpd.protocols.websockets.WebSocket;
+import org.nanohttpd.protocols.websockets.WebSocketFrame;
+import org.nanohttpd.router.RouterNanoHTTPD;
+import org.nanohttpd.util.IHandler;
+import org.slf4j.LoggerFactory;
+
+/**
+ * @author Paul S. Hawke (paul.hawke@gmail.com) On: 4/23/14 at 10:31 PM
+ */
+public class WebServer extends RouterNanoHTTPD{
+    private final static org.slf4j.Logger logger = LoggerFactory.getLogger(WebServer.class);
+
+    /**
+     * logger to log to.
+     */
+    private static final Logger LOG = Logger.getLogger(WebServer.class.getName());
+    private java.util.Timer t=new Timer();
+    private final boolean debug;
+
+    public WebServer(int port, boolean debug) {
+        super(port);
+        this.debug = debug;
+        this.addHTTPInterceptor(new WebServer.Interceptor());
+        addMappings();
     }
 
-    public static void main(String[] args) {
-        ServerRunner.run(WebServer.class);
+    @Override
+    public void addMappings() {
+        super.addMappings();
+        addRoute("/user/:id", BlockHandler.class);
+        addRoute("/interface", UriResponder.class); // this will cause an error
+        addRoute("/toBeDeleted", String.class);
+        removeRoute("/toBeDeleted");
+
+        addRoute("/static(.)+", MyHandler.class, new File("webroot/").getAbsoluteFile());
+        //addRoute("/", StaticPageTestHandler.class, new File("webroot/index.html").getAbsoluteFile());
+    }
+    protected WebSocket openWebSocket(IHTTPSession handshake) {
+        return new MyWebSocket(this, handshake);
     }
 
-    @Override public Response serve(IHTTPSession session) {
-        Map<String, List<String>> decodedQueryParameters =
-                decodeParameters(session.getQueryParameterString());
+    private static class MyWebSocket extends WebSocket {
+        private final WebServer server;
 
-        StringBuilder sb = new StringBuilder();
-        sb.append("<html>");
-        sb.append("<head><title>Debug Server</title></head>");
-        sb.append("<body>");
-        sb.append("<h1>Debug Server</h1>");
+        public MyWebSocket(WebServer server, IHTTPSession handshakeRequest) {
+            super(handshakeRequest);
+            this.server = server;
+        }
 
-        sb.append("<p><blockquote><b>URI</b> = ").append(
-                String.valueOf(session.getUri())).append("<br />");
+        @Override
+        protected void onOpen() {
+            logger.info(String.format("client connected: %s",getHandshakeRequest().getRemoteIpAddress().toString()));
+        }
+
+        @Override
+        protected void onClose(CloseCode code, String reason, boolean initiatedByRemote) {
+            if (server.debug) {
+                System.out.println("C [" + (initiatedByRemote ? "Remote" : "Self") + "] " + (code != null ? code : "UnknownCloseCode[" + code + "]")
+                        + (reason != null && !reason.isEmpty() ? ": " + reason : ""));
+            }
+        }
 
-        sb.append("<b>Method</b> = ").append(
-                String.valueOf(session.getMethod())).append("</blockquote></p>");
+        @Override
+        protected void onMessage(WebSocketFrame message) {
+            try {
+                message.setUnmasked();
+                sendFrame(message);
+            } catch (IOException e) {
+                throw new RuntimeException(e);
+            }
+        }
 
-        sb.append("<h3>Headers</h3><p><blockquote>").
-                append(toString(session.getHeaders())).append("</blockquote></p>");
+        @Override
+        protected void onPong(WebSocketFrame pong) {
+            if (server.debug) {
+                System.out.println("P " + pong);
+            }
+        }
 
-        sb.append("<h3>Parms</h3><p><blockquote>").
-                append(toString(session.getParms())).append("</blockquote></p>");
+        @Override
+        protected void onException(IOException exception) {
+            logger.warn("exception occured", exception);
+        }
 
-        sb.append("<h3>Parms (multi values?)</h3><p><blockquote>").
-                append(toString(decodedQueryParameters)).append("</blockquote></p>");
+        @Override
+        protected void debugFrameReceived(WebSocketFrame frame) {
+            if (server.debug) {
+                System.out.println("R " + frame);
+            }
+        }
 
-        try {
-            Map<String, String> files = new HashMap<String, String>();
-            session.parseBody(files);
-            sb.append("<h3>Files</h3><p><blockquote>").
-                    append(toString(files)).append("</blockquote></p>");
-        } catch (Exception e) {
-            e.printStackTrace();
+        @Override
+        protected void debugFrameSent(WebSocketFrame frame) {
+            if (server.debug) {
+                System.out.println("S " + frame);
+            }
         }
+    }
 
-        sb.append("</body>");
-        sb.append("</html>");
-        return newFixedLengthResponse(sb.toString());
+    public static String makeAcceptKey(String key) throws NoSuchAlgorithmException {
+        MessageDigest md = MessageDigest.getInstance("SHA-1");
+        String text = key + "258EAFA5-E914-47DA-95CA-C5AB0DC85B11";
+        md.update(text.getBytes(), 0, text.length());
+        byte[] sha1hash = md.digest();
+        Base64.Encoder b64encoder = Base64.getEncoder();
+        return b64encoder.encodeToString(sha1hash);
     }
 
-    private String toString(Map<String, ? extends Object> map) {
-        if (map.size() == 0) {
-            return "";
+    private boolean isWebSocketConnectionHeader(Map<String, String> headers) {
+        String connection = (String)headers.get("connection");
+        return connection != null && connection.toLowerCase().contains("Upgrade".toLowerCase());
+    }
+
+    protected boolean isWebsocketRequested(IHTTPSession session) {
+        Map<String, String> headers = session.getHeaders();
+        String upgrade = (String)headers.get("upgrade");
+        boolean isCorrectConnection = this.isWebSocketConnectionHeader(headers);
+        boolean isUpgrade = "websocket".equalsIgnoreCase(upgrade);
+        return isUpgrade && isCorrectConnection;
+    }
+
+    public Response handleWebSocket(IHTTPSession session) {
+        Map<String, String> headers = session.getHeaders();
+        if (this.isWebsocketRequested(session)) {
+            if (!"13".equalsIgnoreCase((String)headers.get("sec-websocket-version"))) {
+                return Response.newFixedLengthResponse(Status.BAD_REQUEST, "text/plain", "Invalid Websocket-Version " + (String)headers.get("sec-websocket-version"));
+            } else if (!headers.containsKey("sec-websocket-key")) {
+                return Response.newFixedLengthResponse(Status.BAD_REQUEST, "text/plain", "Missing Websocket-Key");
+            } else {
+                WebSocket webSocket = this.openWebSocket(session);
+                Response handshakeResponse = webSocket.getHandshakeResponse();
+
+                try {
+                    handshakeResponse.addHeader("sec-websocket-accept", makeAcceptKey((String)headers.get("sec-websocket-key")));
+                } catch (NoSuchAlgorithmException var6) {
+                    return Response.newFixedLengthResponse(Status.INTERNAL_ERROR, "text/plain", "The SHA-1 Algorithm required for websockets is not available on the server.");
+                }
+
+                if (headers.containsKey("sec-websocket-protocol")) {
+                    handshakeResponse.addHeader("sec-websocket-protocol", ((String)headers.get("sec-websocket-protocol")).split(",")[0]);
+                }
+
+                return handshakeResponse;
+            }
+        } else {
+            return null;
+        }
+    }
+
+    protected final class Interceptor implements IHandler<IHTTPSession, Response> {
+        public Interceptor() {
+        }
+        public Response handle(IHTTPSession input) {
+            return WebServer.this.handleWebSocket(input);
         }
-        return unsortedList(map);
     }
 
-    private String unsortedList(Map<String, ? extends Object> map) {
-        StringBuilder sb = new StringBuilder();
-        sb.append("<ul>");
-        for (Map.Entry entry : map.entrySet()) {
-            listItem(sb, entry);
+    public static class BlockHandler extends DefaultHandler {
+        @Override
+        public String getMimeType() {
+            return MIME_PLAINTEXT;
+        }
+
+        @Override
+        public String getText() {
+            return "not implemented";
+        }
+
+        @Override
+        public IStatus getStatus() {
+            return Status.OK;
+        }
+
+        @Override
+        public Response get(UriResource uriResource, Map<String, String> urlParams, IHTTPSession session) {
+            JsonArray blocks = Json.array();
+            return Response.newFixedLengthResponse(blocks.toString());
         }
-        sb.append("</ul>");
-        return sb.toString();
     }
 
-    private void listItem(StringBuilder sb, Map.Entry entry) {
-        sb.append("<li><code><b>").append(entry.getKey()).
-                append("</b> = ").append(entry.getValue()).append("</code></li>");
+    public static class MyHandler extends StaticPageHandler{
+        @Override
+        protected BufferedInputStream fileToInputStream(File fileOrdirectory) throws IOException {
+
+            return new BufferedInputStream(this.getClass().getResourceAsStream(fileOrdirectory.toString()));
+        }
     }
 }

src/main/resources/webroot/index.html

@@ -0,0 +1,3 @@
+<html>
+jo!
+</html>