UDPManager.java

Go to the documentation of this file.

package grl;

import java.nio.ByteBuffer;

import com.kuka.task.ITaskLogger;
import java.io.*;
import java.net.*;

public class UDPManager {
    
    DatagramSocket socket = null;

    ITaskLogger logger;
    String _Remote_IP;
    int _Remote_Port;
    
    InetAddress _address_send = null;
    
    int statesLength = 0;
    long message_counter = 0;
    long noMessageCounter = 0;
    long noMessageCounterLimit = 9999999;
    private grl.flatbuffer.KUKAiiwaStates _currentKUKAiiwaStates = null;
    private grl.flatbuffer.KUKAiiwaState _currentKUKAiiwaState = null;
    private grl.flatbuffer.KUKAiiwaState _previousKUKAiiwaState = null;

    byte[] recBuf = new byte[1400];
    ByteBuffer bb = null;
    boolean stop;

    long startTime;
    long elapsedTime;
    long lastMessageStartTime;
    long lastMessageElapsedTime;
    long lastMessageTimeoutMilliseconds = 1000;
    
    int retriesAllowed = 3;
    int retriesAttempted = 0;
    
    public UDPManager(String laptopIP, String laptopPort, ITaskLogger errorlogger) {
        super();
        this.logger = errorlogger;
        _Remote_IP = laptopIP;
        _Remote_Port = Integer.parseInt(laptopPort);
        
        try {
            _address_send = InetAddress.getByName(_Remote_IP);
        } catch (UnknownHostException e) {
            logger.error("Could not create InetAddress for sending");
        }
        
    }

    /**
     * Blocks until a connection is established or stop() is called.
     * 
     * @return error code: false on success, otherwise failure (or told to stop)
     * @throws UnknownHostException 
     */
    public boolean connect() {

        logger.info("Waiting for connection initialization...");
            
        try {
            socket = new DatagramSocket();
        } catch (SocketException e1) {
            logger.info("failed to create socket.");
        }
        

            
        try {
            socket.setSoTimeout(100);
        } catch (SocketException e1) {
            logger.error("UDPManager failed to set socket timeout");
        }
        
        startTime = System.currentTimeMillis();
        elapsedTime = 0L;
        grl.flatbuffer.KUKAiiwaStates newKUKAiiwaStates = null;
        int newStatesLength = 0; 
        
        boolean connectionEstablished = false;
        
        while(newStatesLength<1 && newKUKAiiwaStates == null){
            
            DatagramPacket packet = new DatagramPacket(recBuf, recBuf.length);
            
            // continues sending dummy messages until the server receives the address of this machine and sends a message back
            while (!connectionEstablished){
                connectionEstablished = preConnect();
                if (stop) {
                    logger.info("Stopping program.");
                    return true; // asked to exit
                }
            }
            
            if(packet.getLength() > 0){

                bb = ByteBuffer.wrap(recBuf);
                
                newKUKAiiwaStates = grl.flatbuffer.KUKAiiwaStates.getRootAsKUKAiiwaStates(bb);
                newStatesLength = newKUKAiiwaStates.statesLength();
        
            }

            if (stop) {
                logger.info("Stopping program.");
                return true; // asked to exit
            }
        }
        
        _currentKUKAiiwaStates = newKUKAiiwaStates;
        statesLength = newStatesLength;

        logger.info("States initialized...");
        
        startTime = System.currentTimeMillis();
        lastMessageStartTime = startTime;
        lastMessageElapsedTime = System.currentTimeMillis() - lastMessageStartTime;
        elapsedTime = 0L;

        return false; // no error
    }
    
    /// @return true successfully sent and received a message, false otherwise
    private boolean preConnect()
    {
        // Dummy message to send to Remote pc (server), in order for the server to know the address of the client (this machine)
        /// @todo Should probably just make this a real flatbuffers init message too
        String dummyMessage = "Hi";
        
        DatagramPacket packetSend= new DatagramPacket(dummyMessage.getBytes(), dummyMessage.getBytes().length, _address_send, _Remote_Port);

        try {
            socket.send(packetSend);
        } catch (IOException e1) {
            // Could not send
        }
        try {
            DatagramPacket packet = new DatagramPacket(recBuf, recBuf.length);
            socket.receive(packet);
            return true;
        } catch (SocketTimeoutException e) {
            // TimeOut reached, continue sending until we receive something 
            return false;
        } catch (IOException e) {
            // Could not receive packet
            return false;
        }
    }

    /**
     * Blocks until a connection is re-established or stop() is called.
     * 
     * @return error code: false on success, otherwise failure (or told to stop)
     * @throws IOException 
     */

    
    public boolean sendMessage(byte[] msg, int size) throws IOException
    {
        DatagramPacket packet= new DatagramPacket(msg, size, _address_send , _Remote_Port );
        socket.send(packet);
        return true;
    }
    
    public grl.flatbuffer.KUKAiiwaState waitForNextMessage()
    {
        boolean haveNextMessage = false;
        while(!stop && !haveNextMessage) {
            
            DatagramPacket packet = new DatagramPacket(recBuf, recBuf.length);
            try {
                socket.receive(packet);

                if(packet.getLength() > 0){

                    message_counter+=1;
                    bb = ByteBuffer.wrap(recBuf);
        
                    _currentKUKAiiwaStates = grl.flatbuffer.KUKAiiwaStates.getRootAsKUKAiiwaStates(bb, _currentKUKAiiwaStates);
        
                    if(_currentKUKAiiwaStates.statesLength()>0) {
                        // initialize the fist state
                        grl.flatbuffer.KUKAiiwaState tmp = _currentKUKAiiwaStates.states(0);
                        if (tmp == null || tmp.armControlState() == null) {
                            noMessageCounter +=1;
                            if (message_counter % 100 == 0) {
                                logger.warn("NULL ArmControlState message, main UDP FlatBuffer message is arriving but doesn't contain any data/commands!");
                            }
                            continue;
                        } else {
                            _previousKUKAiiwaState = _currentKUKAiiwaState;
                            _currentKUKAiiwaState = tmp;
                        }
                            
                        if (_currentKUKAiiwaState == null) {
                            noMessageCounter+=1;
                            logger.error("Missing current state message!");
                            continue;
                        }
                        
                        haveNextMessage=true;
                        noMessageCounter = 0;
                        lastMessageStartTime = System.currentTimeMillis();
                    } else {
                        logger.error("got a UDP packet but it isn't a valid FlatBuffer message, this is an unexpected state that shouldn't occur. please debug me.");
                    }
            //  }
                }
            } catch (IOException e) {
                noMessageCounter +=1;
                if (message_counter % 100 == 0) {
                    logger.warn("Failed to receive UDP packet from control computer... Trying to re-establish connection");
                    preConnect();
                }
            }
            
        }
        
        return _currentKUKAiiwaState;           
    }
    
    public grl.flatbuffer.KUKAiiwaState getCurrentMessage(){
        return _currentKUKAiiwaState;
    }
    
    public grl.flatbuffer.KUKAiiwaState getPrevMessage(){
        return _previousKUKAiiwaState;
    }

    
    
    public boolean isStop() {
        return stop;
    }

    public void setStop(boolean stop) {
        if(stop)
        {
            // done
            socket.close();
            logger.error("socket closed");
            this.stop = stop;
            
        }
    }
    
    public void stop(){
        setStop(true);
    }
    
    protected void finalize() {
        try {
            logger.error("Trying to close socket");
            socket.close();
            logger.error("Socket Closed");
        }
        catch (Exception e)
        {
            logger.error("Could not close socket");
        }
    }

    
    
    
    
    
}