Driver Station Custom UDP (Python/C++)

[Arhowk]

I don't think my plans to switch us back over to Python will work out but in case they do, I'm curious as to if anyone has some insight on an issue we were having regarding UDP DS communcation. (This has nothing to do with the robotpy libs; more of a general networking question). My dashboard is written in C++ and the robot code is in Python. Back in week 2, I'd have some times where I'd plug the robot in and everything would work fine and other times where the two wouldn't talk to each other unless i restarted everything. I think it had something to do with the port getting blocked on the roboRIO side. Does this code have any issues in terms of networking theology

For the code itself its pretty simple, the python should act as a server and listen on port 5805- whenever it receives a connection it will expect the auton mode, defense, and position and will feed back exactly what it was sent. The return packet is than displayed to confirm that the data being communicated is valid.

The entry point for the C++ code is startThread()

Python (RIO side)

    def server_thread_run(self, som,thingy):
        DashComm.print("[DashComm] Starting Server Thread")
        
        import os
        import time
        self.clear()
        while 1:
            self.clear()
            try:
                self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
                DashComm.print("[DashComm] Attempting to obtain socket 5805...")
                self.socket.bind((self.TCP_IP, self.TCP_PORT))
                self.socket.settimeout(5)
                self.socket.listen(0)
                DashComm.print("[DashComm] Successfully obtained socket 5805!")
                try:
                    while 1:
                        self.dataValid = False
                        DashComm.print("[DashComm] Listening for a connection. . .")
                        conn, addr = self.socket.accept()
                        DashComm.print("[DashComm] Found a connection!")
                        while conn and addr:
                            DashComm.print("[DashComm] The connection is valid!")
                            conn.settimeout(3)
                            conn.send(bytes('c' + chr(t) + chr(d) + chr(p), 'ASCII'))
                            data = conn.recv(self.BUFFER_SIZE)
                            DashComm.print("Data Valid: {} {} {} {}".format(data[0], data[1], data[2], data[3]))
                            self.processData(data)
                            self.dataValid = True
                except Exception as ex:
                    self.dataValid = False
                    self.socket.close()
                    time.sleep(2)
            except:
                wpilib.DriverStation.reportError("RESTART THE ROBORIO IF YOU WANT TO USE DS AUTON!!!! SOCKET 5805 OCCUPIED", False)
                #print("Exception : " + ex)
                self.dataValid = False
            finally:
                self.socket.close()
                time.sleep(5)

C++ DriverStation (yes i know QTcpSocket has a really wierd API)

#include "tcpsocketconn.h"
#include <QTcpSocket>

TCPSocketConn::TCPSocketConn(QQuickItem *parent) : QQuickItem(parent)
{

}

void TCPSocketConn::start(){
    qDebug("Start!");
    if(this->_status != -1){
        qDebug("Attempted to start TCPSocketConnection when started? %d", this->_status);
        return;
    }
    _pSocket = new QTcpSocket( this );
    this->_defensePosition = this->_defensePosition & 0xF;
    this->_defenseType = this->_defenseType & 0xF;
    this->_autonMode = this->_autonMode & 0xF;
    qDebug("STart tH!!read");
    connect(_pSocket, SIGNAL(readyRead()), this, SLOT(readTcpData()));
    connect(_pSocket, SIGNAL(connected()), this, SLOT(connected()));
    connect(_pSocket, SIGNAL(disconnected()), this, SLOT(disconnected()));
    connect(_pSocket, SIGNAL(error(QAbstractSocket::SocketError)), this, SLOT(error(QAbstractSocket::SocketError)));

    _pSocket->connectToHost("roborio-1684-frc.local", 5805);

    qDebug("STart tH!!!read");

}

void TCPSocketConn::error(QAbstractSocket::SocketError e){
    qDebug("THREADE RROR");
   //_pSocket->reset();

    this->_defensePosition = this->_defensePosition & 0xF;
    this->_defenseType = this->_defenseType & 0xF;
    this->_autonMode = this->_autonMode & 0xF;
    _pSocket->abort();
    _pSocket->connectToHost("roborio-1684-frc.local", 5805);
}

void TCPSocketConn::startThread(){
    qDebug("STart tHread");
    this->start();
}

void TCPSocketConn::close(){
    qDebug("Close Connection");
}

void TCPSocketConn::readTcpData(){
    //qDebug("Read TP DaTA");
    QByteArray arr = _pSocket->readAll();
    QByteArray r;
    r[0] = 99;
    r[1] = this->_autonMode & 0xF;
    r[2] = this->_defenseType & 0xF;
    r[3] = this->_defensePosition & 0xF;

    //qDebug("Broadcast data %d %d %d", r[1], r[2], r[3]);

    qDebug("Values: %d %d %d %d\n",(int) arr[0], (int) arr[1], (int) arr[2], (int) arr[3]);
    if((int)arr[0] == 99){
        this->_autonMode =       ((int)arr[1]) << 4 + (this->_autonMode & 0xF);
        this->_defenseType =     ((int)arr[2]) << 4 + (this->_defenseType & 0xF);
        //qDebug("How is defense type %d %d %d", (int)arr[2], (int)arr[2] << 4, this->_defenseType);
        this->_defensePosition = ((int)arr[3]) << 4 + (this->_defensePosition & 0xF);
    }

    _pSocket->write(r);
}

void TCPSocketConn::disconnected(){
    qDebug("disconnected");
    //_pSocket->reset();

    this->_defensePosition = this->_defensePosition & 0xF;
    this->_defenseType = this->_defenseType & 0xF;
    this->_autonMode = this->_autonMode & 0xF;
    _pSocket->connectToHost("roborio-1684-frc.local", 5805);
}

void TCPSocketConn::connected(){
    qDebug("Connected");
}

QString TCPSocketConn::url(){
    return this->_url;
}

void TCPSocketConn::url(QString s){
    this->_url = s;

    if(this->_status == -1){
        this->startThread();
    }
}


QString TCPSocketConn::message(){
    return this->_lastMessage;
}

int TCPSocketConn::status(){
    return this->_status;
}

//the least significant byte is what the user requests
//the most significant byte is what the robot thinks it is
int TCPSocketConn::defensePosition(){

    return (this->_defensePosition & 0xF0) >> 4;
}

int TCPSocketConn::defenseType(){
    return (this->_defenseType & 0xF0) >> 4;
}

int TCPSocketConn::autonMode(){
    return (this->_autonMode & 0xF0) >> 4;
}

void TCPSocketConn::defensePosition(int x){
    this->_defensePosition = (this->_defensePosition & 0xF0) + (x & 0x0F);
}

void TCPSocketConn::defenseType(int x){
    this->_defenseType = (this->_defenseType & 0xF0) + (x & 0x0F);
}

void TCPSocketConn::autonMode(int x){
    this->_autonMode = (this->_autonMode & 0xF0) + (x & 0x0F);
}




void TCPSocketConn::a_3(QString d){

}
void TCPSocketConn::a_4(int s){

}