Browse Source

fixes reconnect issues (evva on-site)

Martin Kunz 2 years ago
parent
commit
471130803e

+ 5 - 5
src/main/java/at/acdp/urweb/Main.java

@@ -1,5 +1,6 @@
 package at.acdp.urweb;
 
+import at.acdp.urweb.fhpp.FHMaster;
 import at.acdp.urweb.fhpp.FHMasterSim;
 import at.acdp.urweb.fhpp.IFHMaster;
 import at.acdp.urweb.rtde.RTDEClient;
@@ -12,7 +13,7 @@ import org.slf4j.LoggerFactory;
 
 public class Main {
     private final static org.slf4j.Logger logger = LoggerFactory.getLogger(Main.class);
-    public static IFHMaster fhm=new FHMasterSim();
+    public static IFHMaster fhm=new FHMaster();
     public static void main(String[] args) {
         Params app = null;
         try {
@@ -23,17 +24,16 @@ public class Main {
             NanoServer server = new NanoServer("http://0.0.0.0:" + app.port);
 
             RTDEClient r=new RTDEClient("127.0.0.1", 30004);
-            r.start();
             var so=new RtdeSetupOutputs()
                     .addVariable("timestamp")
                     .addVariable("robot_mode")
                     .addVariable("safety_mode")
                     .addVariable("safety_status")
                     .addVariable("target_speed_fraction")
+                    .addVariable("runtime_state")
                     .addVariable("speed_scaling");
-            r.request(so);
-            var start=new RtdeControlStart();
-            r.request(start);
+            r.start(so);
+
 
             PosThread p=new PosThread(r, fhm);
             p.start();

+ 3 - 2
src/main/java/at/acdp/urweb/PosThread.java

@@ -17,6 +17,7 @@ public class PosThread implements Runnable{
     private final IFHMaster fhm;
     private Thread thread;
     private volatile boolean _running=true;
+    public static Status lastStatus;
 
     public PosThread(RTDEClient r, IFHMaster fhm) {
         this.rtde=r;
@@ -36,9 +37,9 @@ public class PosThread implements Runnable{
             rtde.request(si);
             while(_running) {
                 CompletableFuture<Status> s = fhm.readStatus();
-                Status ss=s.join();
+                lastStatus=s.join();
                 var pos=new RtdeInDataPackage(si);
-                pos.put("input_int_register_24", String.valueOf(ss.istPosition));
+                pos.put("input_int_register_24", String.valueOf(lastStatus.istPosition));
                 rtde.request(pos);
                 Thread.sleep(100);
             }

+ 2 - 2
src/main/java/at/acdp/urweb/fhpp/FHMaster.java

@@ -62,10 +62,10 @@ public class FHMaster implements IFHMaster {
     }
 
     @Override
-    public void direktAuftrag(int pos, int speed, int start, boolean relative) {
+    public void direktAuftrag(int pos, int speed, int start, boolean relative, boolean stop) {
         byte ccon=0x0;
         ccon|=(1<<0); // Enable drive
-        ccon|=(1<<1); // !Stop
+        ccon|=((stop?0:1)<<1); // !Stop
         ccon|=(1<<2); // Release Brake
         ccon|=(0<<3); //Reset fault
         ccon|=(0<<4); //Reserved

+ 1 - 1
src/main/java/at/acdp/urweb/fhpp/FHMasterSim.java

@@ -31,7 +31,7 @@ public class FHMasterSim implements IFHMaster {
     }
 
     @Override
-    public void direktAuftrag(int pos, int speed, int start, boolean relative) {
+    public void direktAuftrag(int pos, int speed, int start, boolean relative, boolean stop) {
         logger.info("direktAuftrag {},{},{},{}", pos, speed, start, relative);
         targetPos=pos;
     }

+ 1 - 1
src/main/java/at/acdp/urweb/fhpp/IFHMaster.java

@@ -7,7 +7,7 @@ import java.util.concurrent.CompletableFuture;
 public interface IFHMaster {
     void start(Params app) throws InterruptedException;
 
-    void direktAuftrag(int pos, int speed, int start, boolean relative);
+    void direktAuftrag(int pos, int speed, int start, boolean relative, boolean stop);
 
     CompletableFuture<Status> readStatus();
 

+ 82 - 42
src/main/java/at/acdp/urweb/rtde/RTDEClient.java

@@ -15,19 +15,20 @@ import static at.acdp.urweb.rtde.CommandType.*;
 public class RTDEClient implements Runnable {
     private final static org.slf4j.Logger logger = LoggerFactory.getLogger(RTDEClient.class);
     private final Semaphore waitingRequestSem = new Semaphore(0);
-    private final Semaphore socketOpen = new Semaphore(0);
-
     private volatile IRtdeData waitingRequest=null;
 
     public static int RTDE_PROTOCOL_VERSION = 2;
     private final String ip;
     private final int port;
-    private final boolean _running = true;
-    private DataOutputStream dos;
-    private DataInputStream dis;
+    private volatile boolean _running = true;
+    private volatile boolean initDone = false;
+
+    private volatile DataOutputStream dos;
+    private volatile DataInputStream dis;
     private Thread readThread;
     private RtdeSetupOutputs outputs;
     private HashMap<String, String> lastData;
+    private RtdeSetupOutputs so;
 
 
     public RTDEClient(String ip, int port) {
@@ -35,11 +36,11 @@ public class RTDEClient implements Runnable {
         this.port = port;
     }
 
-    public void start() throws IOException, InterruptedException {
+    public void start(RtdeSetupOutputs so) throws IOException, InterruptedException {
+        this.so=so;
         readThread = new Thread(this);
         readThread.start();
-        socketOpen.acquire();
-        init();
+
     }
 
     public HashMap<String, String> getLastData() {
@@ -47,6 +48,10 @@ public class RTDEClient implements Runnable {
     }
 
     public void request(IRtdeData data) throws IOException, InterruptedException {
+        while(dos==null||dis==null) {
+            Thread.sleep(100);
+        }
+
         if(readThread.getId()==Thread.currentThread().getId()) {
             logger.error("request() must not be called from readThread.");
         }
@@ -61,16 +66,57 @@ public class RTDEClient implements Runnable {
         return;
     }
 
-    // Internal method that actually reads the data
-    private void readSocket() throws IOException, InterruptedException {
-        int length = dis.readShort();
-        int type=dis.readByte() & 0xFF;
+
+    private boolean init() throws IOException, InterruptedException {
+        {
+            var rpv = new RtdeRequestProtocolVersion();
+            rpv.send(dos);
+            int length = dis.readShort();
+            int type = dis.readByte() & 0xFF;
+            if(type!=RTDE_REQUEST_PROTOCOL_VERSION)
+                return false;
+            rpv.read(dis, length - 3);
+            logger.info("RtdeRequestProtocolVersion succes:" + rpv.success);
+        }
+        {
+            var ruv = new RtdeRequestURVersion();
+            ruv.send(dos);
+            int length=0;
+            length = dis.readShort();
+            int type = dis.readByte() & 0xFF;
+            if(type!=RTDE_GET_URCONTROL_VERSION)
+                return false;
+            ruv.read(dis, length - 3);
+            logger.info(String.format("RtdeRequestURVersion: %d.%d.%d.%d",ruv.major, ruv.minor, ruv.build, ruv.bugfix));
+        }
+        {
+            so.send(dos);
+            int length = dis.readShort();
+            int type = dis.readByte() & 0xFF;
+            if(type!=RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS)
+                return false;
+            var r=so.read(dis, length - 1);
+            logger.info("so:" +r.typesText);
+            outputs=r;
+        }
+        {
+            var start=new RtdeControlStart();
+            start.send(dos);
+            int length = dis.readShort();
+            int type = dis.readByte() & 0xFF;
+            if(type!=RTDE_CONTROL_PACKAGE_START)
+                return false;
+            start.read(dis, length -1);
+            logger.info("start accepted:" +start.accepted);
+            initDone=true;
+        }
+        return true;
+    }
+
+    private void handlePaket(int type, int length) throws IOException {
         var wreq=waitingRequest;
         if(wreq != null && type==wreq.getType()) {
             waitingRequest.read(dis, length-3);
-            if(type== RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS) {
-                outputs= (RtdeSetupOutputs) waitingRequest;
-            }
             waitingRequestSem.release();
         } else switch(type) {
             case RTDE_TEXT_MESSAGE:
@@ -95,37 +141,31 @@ public class RTDEClient implements Runnable {
         }
     }
 
+
     @Override
     public void run() {
-        try {
-            Socket rt = new Socket(ip, port);
-            rt.setSoTimeout(0);
-            rt.setReuseAddress(true);
-            rt.setTcpNoDelay(true);
-            if (rt.isConnected()) {
-                System.out.println("Connected to UR Realtime Client");
-            }
-            dos = new DataOutputStream(rt.getOutputStream());
-            dis = new DataInputStream(rt.getInputStream());
-            socketOpen.release();
-            while (_running) {
-                try {
-                    readSocket();
-                } catch (IOException | InterruptedException e) {
-                    e.printStackTrace();
+        while (_running) {
+            initDone=false;
+            try (Socket rt = new Socket(ip, port)){
+                rt.setSoTimeout(10000);
+                rt.setReuseAddress(true);
+                rt.setTcpNoDelay(true);
+                if (rt.isConnected()) {
+                    logger.info("Connected to UR Realtime Client");
+                    dos = new DataOutputStream(rt.getOutputStream());
+                    dis = new DataInputStream(rt.getInputStream());
+                    if(!init())
+                        throw new IOException("init failed");
+                    while (_running) {
+                        int length = dis.readShort();
+                        int type=dis.readByte() & 0xFF;
+                        handlePaket(type, length);
+                    }
                 }
+            } catch (IOException| InterruptedException e) {
+                logger.warn("rtde connection closed", e);
+
             }
-        } catch (IOException  e) {
-            logger.warn("run",e);
         }
     }
-
-    public void init() throws IOException, InterruptedException {
-        var rpv = new RtdeRequestProtocolVersion();
-        request(rpv);
-        logger.info("RtdeRequestProtocolVersion succes:" + rpv.success);
-        var ruv = new RtdeRequestURVersion();
-        request(ruv);
-        logger.info(String.format("RtdeRequestURVersion: %d.%d.%d.%d",ruv.major, ruv.minor, ruv.build, ruv.bugfix));
-    }
 }

+ 1 - 1
src/main/java/at/acdp/urweb/rtde/packets/RtdeSetupOutputs.java

@@ -31,7 +31,7 @@ public class RtdeSetupOutputs implements IRtdeData {
     @Override
     public RtdeSetupOutputs read(DataInputStream d, int length) throws IOException {
         recipe = d.readByte();
-        byte[] text=new byte[length-1];
+        byte[] text=new byte[length-3];
         d.readFully(text);
         typesText=new String(text);
         types.addAll(Arrays.asList(typesText.split(",")));

+ 31 - 21
src/main/java/at/acdp/urweb/web/FestoXRServer.java

@@ -1,7 +1,9 @@
 package at.acdp.urweb.web;
 import at.acdp.urweb.Main;
+import at.acdp.urweb.PosThread;
 import at.acdp.urweb.fhpp.Status;
 import at.acdp.urweb.rtde.RTDEClient;
+import ch.qos.logback.core.pattern.PostCompileProcessor;
 import com.nmote.xr.XRMethod;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
@@ -19,17 +21,17 @@ public class FestoXRServer {
     }
 
     @XRMethod(value = "example.helloWorld", help = "Returns 'Helo ' + argument")
-    public static String hello(Object s) {
+    public String hello(Object s) {
         return "Hello '" + s + "'";
     }
 
     @XRMethod(value = "ref2", help = "ref")
-    public static String ref(String a, String b) {
+    public String ref(String a, String b) {
         return "ref";
     }
 
     @XRMethod(value = "rel", help = "rel")
-    public static String rel(int relPos, int speed) {
+    public String rel(int relPos, int speed) {
         try {
             logger.info("rel  {}", relPos);
             int curPos=Main.fhm.readStatus().get().istPosition;
@@ -40,9 +42,9 @@ public class FestoXRServer {
                 logger.info("Controller not enabled");
                 return "Controller not enabled";
             }
-            Main.fhm.direktAuftrag(relPos, speed,0, true);
+            Main.fhm.direktAuftrag(relPos, speed,0, true, false);
             Thread.sleep(10);
-            Main.fhm.direktAuftrag(relPos, speed, 1, true);
+            Main.fhm.direktAuftrag(relPos, speed, 1, true, false);
             Thread.sleep(10);
 
             while(true) {
@@ -60,30 +62,38 @@ public class FestoXRServer {
     }
 
     @XRMethod(value = "abs", help = "abs")
-    public static String abs(int targetPos, int speed) {
+    public String abs(int targetPos, int speed) {
         try {
             int round=1;
+            boolean stopped=false;
             while(true) {
-                logger.info("Absolute to {} speed {} try {}", targetPos, speed, round++);
-                Main.fhm.bereitschaft();
-                Main.fhm.direktAuftrag(targetPos, speed, 0, false);
-                Thread.sleep(100);
-                Main.fhm.direktAuftrag(targetPos, speed, 1, false);
-
+                logger.info("Absolute to {} speed {} try {}, stopped {}", targetPos, speed, round++, stopped);
+                if(!stopped) {
+                    Main.fhm.bereitschaft();
+                    Main.fhm.direktAuftrag(targetPos, speed, 0, false, false);
+                    Thread.sleep(100);
+                    Main.fhm.direktAuftrag(targetPos, speed, 1, false, false);
+                }
                 while (true) {
                     boolean reached = checkPos(targetPos);
                     Status s = Main.fhm.readStatus().get();
-                    if (s.fault) {
-                        logger.warn("fault, retry");
-                        break;
-                    }
                     if (reached) {
                         logger.info("Reached {}", targetPos);
                         return "OK";
                     }
-                    Thread.sleep(100);
-                }
-            }
+                    int rs=Integer.parseInt(rtde.getLastData().get("runtime_state"));
+                    if(rs!=2 && rs!=5) {
+                        Main.fhm.direktAuftrag(targetPos, speed, 0, false, false);
+                        Thread.sleep(10);
+                        Main.fhm.direktAuftrag(targetPos, speed, 0, false, true);
+                        Thread.sleep(100);
+                        stopped=true;
+                        break;
+                    } else if(stopped==true){
+                        stopped=false;
+                        break;
+                    }
+                }}
         } catch (ExecutionException|InterruptedException e) {
             logger.warn("abs failed", e);
             return "abs failed: "+e.toString();
@@ -91,7 +101,7 @@ public class FestoXRServer {
     }
 
     @XRMethod(value = "getpos", help = "getpos")
-    public static int getpos() {
+    public int getpos() {
         logger.info("GetPos");
         try {
             return Main.fhm.readStatus().get().istPosition;
@@ -104,7 +114,7 @@ public class FestoXRServer {
     }
 
     @XRMethod(value = "ready", help = "ready")
-    public static String ready() {
+    public String ready() {
         try {
             logger.info("Ready called");
             Main.fhm.bereitschaft();