class OCUCommand(QObject):
def __init__(self, target, port=9000):
super(OCUCommand, self).__init__(None)
self.socket = QUdpSocket(self)
self.socket.bind(QHostAddress('192.168.10.10'), port)
self.port = port
self.target = QHostAddress(target)
self.emergency_led = 0
def send(self):
datagram = QByteArray()
stream = QDataStream(datagram, QIODevice.WriteOnly)
stream.writeUInt8(self.emergency_led)
self.socket.writeDatagram(datagram, self.target, self.port)
def __init__(self, **kwargs):
super().__init__(**kwargs)
ping_message = create_message(SSDPInterface.ADDRESS, SSDPInterface.PORT, 2, SSDPInterface.SERVICE)
self.broadcast_datagram = QNetworkDatagram(QByteArray(ping_message),
QHostAddress(SSDPInterface.ADDRESS),
SSDPInterface.PORT)
self.udp_sockets = socks = []
for host_address in QNetworkInterface.allAddresses():
if host_address.isLoopback() or host_address.protocol() != QAbstractSocket.IPv4Protocol:
continue
sock = QUdpSocket(self)
sock.bind(host_address, 50000)
# lambdas will not work in this case as they are late binding.
func = partial(self.packet_received, sock)
sock.readyRead.connect(func)
socks.append(sock)
print(f"Connected to interface at host address: {host_address.toString()}")
class HttpSignalsCase(unittest.TestCase):
'''Test case for bug #124 - readDatagram signature
QUdpSocket.readDatagram must return a tuple with the datagram, host and
port, while receiving only the max payload size.'''
def setUp(self):
#Acquire resources
self.called = False
self.app = QCoreApplication([])
self.socket = QUdpSocket()
self.server = QUdpSocket()
self.server.bind(QHostAddress(QHostAddress.LocalHost), 45454)
def tearDown(self):
#Release resources
del self.socket
del self.server
del self.app
def sendPackage(self):
addr = QHostAddress(QHostAddress.LocalHost)
self.socket.writeDatagram('datagram', addr, 45454)
def callback(self):
while self.server.hasPendingDatagrams():
datagram, host, port = self.server.readDatagram(
self.server.pendingDatagramSize())
self.called = True
self.app.quit()
def testDefaultArgs(self):
#QUdpSocket.readDatagram pythonic return
# @bug 124
QObject.connect(self.server, SIGNAL('readyRead()'), self.callback)
self.sendPackage()
self.app.exec_()
self.assertTrue(self.called)
class HttpSignalsCase(unittest.TestCase):
'''Test case for bug #124 - readDatagram signature
QUdpSocket.readDatagram must return a tuple with the datagram, host and
port, while receiving only the max payload size.'''
def setUp(self):
#Acquire resources
self.called = False
self.app = QCoreApplication([])
self.socket = QUdpSocket()
self.server = QUdpSocket()
self.server.bind(QHostAddress(QHostAddress.LocalHost), 45454)
def tearDown(self):
#Release resources
del self.socket
del self.server
del self.app
def sendPackage(self):
addr = QHostAddress(QHostAddress.LocalHost)
self.socket.writeDatagram('datagram', addr, 45454)
def callback(self):
while self.server.hasPendingDatagrams():
datagram, host, port = self.server.readDatagram(self.server.pendingDatagramSize())
self.called = True
self.app.quit()
def testDefaultArgs(self):
#QUdpSocket.readDatagram pythonic return
# @bug 124
QObject.connect(self.server, SIGNAL('readyRead()'), self.callback)
self.sendPackage()
self.app.exec_()
self.assert_(self.called)
class Example(QWidget):
def get_field(self, data, offset, size):
index = 0
field = ''
while (index < size):
field += data[offset + index]
index += 1
return field
def __init__(self):
super().__init__()
self.layout = QVBoxLayout()
self.initUI()
self.udpSocket = QUdpSocket()
self.udpSocket.bind(QHostAddress('127.0.0.1'), 7111)
self.udpSocket.readyRead.connect(self.readPendingDatagrams)
def initUI(self):
''' Initialize the User Interface '''
self.setGeometry(300, 300, 300, 220)
self.setWindowTitle('Test App')
self.addButtons()
self.setLayout(self.layout)
self.show()
def addButtons(self):
self.layout.addWidget(
QLabel('<H1><font color="red">Hello PyQt5</font></H1>'))
buttonList = []
numbers = ('one', 'two', 'three', 'four')
for i in range(4):
button = QPushButton(numbers[i])
self.layout.addWidget(button)
buttonList.insert(i, button)
buttonList[i].clicked.connect(self.buttonClick)
#self.layout.addWidget(QPushButton('Button ' + str(i)))
self.line = QLineEdit()
self.layout.addWidget(self.line)
self.label = QLabel()
self.layout.addWidget(self.label)
self.line.textChanged.connect(self.label.setText)
def buttonClick(self):
button = self.sender()
if isinstance(button, QPushButton):
text = f'Button {button.text()}'
print('button click event from ' + button.text())
self.line.setText('')
#datagram = QNetworkDatagram(bytes(text, 'utf-8'), QHostAddress.LocalHost, 45454)
#self.udpSocket.writeDatagram(datagram)
datagram = QByteArray(bytes(text, 'utf-8'))
self.udpSocket.writeDatagram(datagram, QHostAddress('127.0.0.1'),
6010)
def readPendingDatagrams(self):
while self.udpSocket.hasPendingDatagrams():
# datagram = QByteArray()
# datagram.resize(self.udpSocket.pendingDatagramSize())
(datagram, sender, senderPort) = self.udpSocket.readDatagram(1024)
print(f'{sender.toString()}:{senderPort}')
self.processDatagram(datagram)
def processDatagram(self, datagram):
pkt = list(datagram)
# print(pkt)
for i in range(0, len(pkt)):
hex_val = hex(int.from_bytes(pkt[i], byteorder='big'))[2:]
hex_val = hex_val.rjust(2, '0')
# pkt[i] = str(hex_val)
pkt[i] = chr(int(hex_val, 16))
print(pkt)
# msg_id = int(self.get_field(pkt, 14, 4), 16)
# print(f'Message: {msg_id}')
def closeEvent(self, event):
pass
class NetworkConnectionClass(QtCore.QObject):
SignalDataRec = Signal(str, int)
SignalFrameDataAvailable = Signal()
def __init__(self, parent=None):
super(NetworkConnectionClass, self).__init__(parent=parent)
#self.CommandSocket = QTcpSocket()
self.CommandSocket = QUdpSocket()
self.CommandSocket.bind(QHostAddress("192.168.100.2"), 2323)
self.CommandSocket.connected.connect(self.EventConnectedHandle)
self.CommandSocket.readyRead.connect(self.RecieveData)
#self.CommandSocket.connectToHost("192.168.100.5",2323,QIODevice.ReadWrite)
#self.CommandSocket.connectToHost("127.0.0.1",2323,QIODevice.ReadWrite)
#self.CommandSocket.connectToHost("192.168.20.196",2323,QIODevice.ReadWrite)
#print("NETWORK - ",self.CommandSocket.localAddress(),self.CommandSocket.peerAddress())
self.Display = WindowNetworkConnection()
self.NetworkDataArray = QByteArray()
self.NetworkDataArray.resize(2000)
self.SignalDataRec.connect(self.Display.PrintData)
self.BufferWrite = QBuffer(self.NetworkDataArray)
self.BufferWrite.open(QBuffer.WriteOnly)
self.BufferRead = QBuffer(self.NetworkDataArray)
self.BufferRead.open(QBuffer.ReadOnly)
self.ReadDataStream = QDataStream(self.BufferRead)
self.ReadDataStream.setByteOrder(QDataStream.LittleEndian)
self.WriteDataStream = QDataStream(self.BufferWrite)
self.WriteDataStream.setByteOrder(QDataStream.LittleEndian)
self.SocketDataStream = QDataStream(self.CommandSocket)
self.SocketDataStream.setByteOrder(QDataStream.LittleEndian)
self.Timer = QTime()
self.Timer.start()
self.BufferRead.seek(0)
self.BufferWrite.seek(0)
self.LimitBufferSize = 2000
self.MinTransferUnit = 7
self.MaxTransferUnit = 18
self.bytesAvailable = 0
self.Display.ui.pushButton.clicked.connect(self.SendData)
#===================================================== WRITE BUFFER
self.SendBuffer = QByteArray()
#=====================================================
# TestMessage = QByteArray(bytearray.fromhex('A1 A2 A3')); TestMessage2 = QByteArray(bytearray.fromhex('A4 A5 A6'))
# self.BufferWriteQueue.write(TestMessage); self.BufferWriteQueue.write(TestMessage2)
# print("BUFFER - ",self.NetworkDataArray)
# self.Message = MessageData()
# self.Message << self.NetworkDataStream << self.NetworkDataStream
def __rshift__(self, Reciever: DataTransferHeader):
Reciever << self.ReadDataStream
self.bytesAvailable -= Reciever.UNIT_SIZE
print("BYTES AVAILABLE - ", self.bytesAvailable)
#print(" REABUFFER POS - ",self.BufferRead.pos(),'BYTES AVAILABLE - ',self.bytesAvailable,"UNIT SIZE -",Reciever.UNIT_SIZE)
#print(" READ UNIT -",Reciever.UNIT_SIZE)
#print(" READ BUFFER POS -",self.BufferRead.pos())
#print(" READ BUFFER -",self.BufferRead.data().toHex())
if not self.BufferRead.pos(
) < self.LimitBufferSize - self.MaxTransferUnit:
self.BufferRead.seek(0)
print("READ BUFFER SEEK TO 0 - ", self.BufferRead.pos())
return self
def EventConnectedHandle(self):
self.SignalDataRec.emit("CONNECTED TO HOST", 0)
#self.SignalDataRec.emit("192.168.100.5 PORT 2323",0)
self.SignalDataRec.emit("192.168.20.197 PORT 2323", 0)
#self.SignalDataRec.emit("BYTES IN REC BUFFER - " + str(self.bytesAvailable))
def RecieveData(self):
#self.CommandSocket.receiveDatagram(20)
HEADER = QByteArray()
HEADER.resize(2)
if self.bytesAvailable < self.LimitBufferSize - self.MaxTransferUnit: #check that memory for packet available
if self.BufferWrite.pos(
) < self.LimitBufferSize - self.MaxTransferUnit:
(newData, sender,
senderPort) = self.CommandSocket.readDatagram(20)
#newData = self.CommandSocket.read(self.LimitBufferSize - self.bytesAvailable)
#print(" NEW DATA - ",newData.toHex(),"to POS - ",self.BufferWrite.pos())
self.BufferWrite.write(newData)
self.bytesAvailable += newData.size()
print(self.Timer.restart())
#print("REC ",data)
#print(" BUFFER - ",self.BufferWrite.data().toHex())
else:
H1 = 0
H2 = 0
while self.CommandSocket.bytesAvailable() > 0:
H1 = H2
H2 = self.SocketDataStream.readInt16()
if H1 == 0xF1 and (H2 == 0xD1 or H2 == 0xD2 or H2 == 0xD3
or H2 == 0xC1):
self.BufferWrite.seek(0)
newData = self.CommandSocket.read(
self.LimitBufferSize - self.bytesAvailable)
self.WriteDataStream.writeInt16(H1)
self.WriteDataStream.writeInt16(H2)
print(
"=============================================================="
)
print("SEEK TO 0")
print("NEW DATA - ", newData, "to POS - ",
self.BufferWrite.pos() - 4)
self.BufferWrite.write(newData)
self.bytesAvailable += (4 + newData.size())
#print("BUFFER - ",self.NetworkDataArray.toHex())
break
else:
self.WriteDataStream.writeInt16(H2)
self.bytesAvailable += 1
#self.SignalDataRec.emit("BYTES IN REC BUFFER - " + str(self.bytesAvailable))
if (self.bytesAvailable >= self.MinTransferUnit):
self.SignalFrameDataAvailable.emit()
def SendData(self):
Req = ConnectCheckRequest()
self.SendBuffer.clear()
self.SendBuffer.resize(Req.MESSAGE_SIZE)
WriteDataStream = QDataStream(self.SendBuffer, QIODevice.ReadWrite)
WriteDataStream.setByteOrder(QDataStream.LittleEndian)
Req >> WriteDataStream
self.CommandSocket.write(self.SendBuffer)
self.CommandSocket.waitForBytesWritten(20)
self.SignalDataRec.emit("CHECK CONNECT - " + str(Req.Connect), 1)
#self.CommandSocket.write(bytearray(b'TestMessage\r\n'))
#print("WRITE BUFFER - ",self.SendBuffer.toHex())
#print("===============================================")
def HandleErrorSocket(self):
print("ErrorSocket")
class _ManiPulatorProtocol(QObject):
def __init__(self, controller_ip='192.168.10.10',main_board_ip='192.168.10.170', mainboard_port='3030', sensor_board_ip='192.168.10.20', sensor_board_port='3032'):
super(_ManiPulatorProtocol, self).__init__(None)
self.direction = Direction()
self.angularspeed = AngularSpeed()
self.reset_motor = ResetMotor()
self.home_position = HomePosition()
self.emergency_stop = EmergencyStop()
self.set_led = SetLED()
# TO DO init should call from class
self.init_sockets(controller_ip)
self.main_board_ip = QHostAddress(main_board_ip)
self.mainboard_port = int(mainboard_port)
self.sensor_board_ip = QHostAddress(sensor_board_ip)
self.sensor_board_port = int(sensor_board_port)
def set_zero(self):
self.direction = Direction()
self.angularspeed = AngularSpeed()
def init_sockets(self,controller_ip):
# self.socket_main_board = QUdpSocket(self)
self.socket_sensor_board = QUdpSocket(self)
self.socket_old_main_board = QUdpSocket(self)
self.socket_sensor_board.bind(QHostAddress(controller_ip), 3037)
self.socket_old_main_board.bind(QHostAddress(controller_ip), 3030)
def send_data_old_main_board(self):
datagram = QByteArray()
stream = QDataStream(datagram, QIODevice.WriteOnly)
# print(self.angularspeed.joint1)
low_byte1, high_byte1 = two_byte_int_to_byte(self.angularspeed.joint1)
low_byte2, high_byte2 = two_byte_int_to_byte(self.angularspeed.joint2)
low_byte3, high_byte3 = two_byte_int_to_byte(self.angularspeed.joint3)
low_byte4, high_byte4 = two_byte_int_to_byte(
self.angularspeed.prismatic)
low_byte5, high_byte5 = two_byte_int_to_byte(self.angularspeed.joint4)
low_byte6, high_byte6 = two_byte_int_to_byte(self.angularspeed.joint5)
low_byte7, high_byte7 = two_byte_int_to_byte(
self.angularspeed.sensor_box)
stream.writeInt8(self.direction.joint1)
stream.writeInt8(self.direction.joint2)
stream.writeInt8(self.direction.joint3)
stream.writeInt8(self.direction.prismatic)
stream.writeInt8(self.direction.joint4)
stream.writeInt8(self.direction.joint5)
stream.writeInt8(self.direction.sensor_box)
stream.writeUInt8(low_byte1)
stream.writeUInt8(high_byte1)
stream.writeUInt8(low_byte2)
stream.writeUInt8(high_byte2)
stream.writeUInt8(low_byte3)
stream.writeUInt8(high_byte3)
stream.writeUInt8(low_byte4)
stream.writeUInt8(high_byte4)
stream.writeUInt8(low_byte5)
stream.writeUInt8(high_byte5)
stream.writeUInt8(low_byte6)
stream.writeUInt8(high_byte6)
stream.writeUInt8(low_byte7)
stream.writeUInt8(high_byte7)
stream.writeUInt8(self.reset_motor.reset_main_board_motor)
stream.writeUInt8(self.home_position.home_position)
stream.writeUInt8(self.emergency_stop.emergency_stop)
self.socket_old_main_board.writeDatagram(
datagram, self.main_board_ip, self.mainboard_port)
def send_sensor_board_manipulator_data(self):
datagram = QByteArray()
stream = QDataStream(datagram, QIODevice.WriteOnly)
low_byte4, high_byte4 = two_byte_int_to_byte(self.angularspeed.joint4)
stream.writeUInt8(self.direction.joint4)
stream.writeUInt8(self.direction.joint5)
stream.writeUInt8(self.direction.gripper)
stream.writeUInt8(self.direction.sensor_box)
stream.writeUInt8(low_byte4)
stream.writeUInt8(high_byte4)
stream.writeUInt8(self.reset_motor.reset_sensor_board_motor)
stream.writeUInt8(self.set_led.set_led)
self.socket_sensor_board.writeDatagram(
datagram, self.sensor_board_ip, 3032)
def send_main_board_manipulator_data(self):
datagram = QByteArray()
stream = QDataStream(datagram, QIODevice.WriteOnly)
low_byte1, high_byte1 = two_byte_int_to_byte(self.angularspeed.joint1)
low_byte2, high_byte2 = two_byte_int_to_byte(self.angularspeed.joint2)
low_byte3, high_byte3 = two_byte_int_to_byte(self.angularspeed.joint3)
stream.writeUInt8(self.direction.joint1)
stream.writeUInt8(self.direction.joint2)
stream.writeUInt8(self.direction.joint3)
stream.writeUInt8(self.direction.prismatic)
stream.writeUInt8(low_byte1)
stream.writeUInt8(high_byte1)
stream.writeUInt8(low_byte2)
stream.writeUInt8(high_byte2)
stream.writeUInt8(low_byte3)
stream.writeUInt8(high_byte3)
stream.writeUInt8(self.reset_motor.reset_main_board_motor)
stream.writeUInt8(self.home_position.home_position)
stream.writeUInt8(self.emergency_stop.emergency_stop)
self.socket_main_board.writeDatagram(
datagram, self.main_board_ip, self.mainboard_port)
class NodeHandler(QObject):
newNodeDiscovered = Signal(dict)
nodeConnected = Signal(dict)
nodeDisconnected = Signal(dict)
nodeDisappeared = Signal(dict)
nodeAliveMessage = Signal(dict)
cleanupDone = Signal()
threadReady = Signal()
def __init__(self, mainwindow):
super(NodeHandler, self).__init__()
self.visibleNodes = {}
self.connectedNodes = {}
self.nodeListMutex = QMutex()
self.mainwindow = mainwindow
@Slot()
def onRun(self):
# multithreading hack to prevent threading sigsev conditions
# all the stuff should be executed in this threads event loop
# if we do stuff here, the thread context could be different
self.threadReady.connect(self.onThreadReady)
self.threadReady.emit()
@Slot()
def onThreadReady(self):
self.udpSocket = QUdpSocket(self)
#self.udpSocket.bind(13370, QUdpSocket.ShareAddress)
self.udpSocket.bind(13370, QUdpSocket.ReuseAddressHint)
self.udpSocket.readyRead.connect(self.onSocketReadyRead)
# check every second
self.disconnectTimer = QTimer(self)
self.disconnectTimer.moveToThread(self.thread())
self.disconnectTimer.timeout.connect(self.onDisconnectTimerFire)
self.disconnectTimer.start(100)
@Slot()
def onSocketReadyRead(self):
msg = self.udpSocket.readDatagram(self.udpSocket.pendingDatagramSize())
if msg[0][0:2] == "CB":
msg_split = msg[0].split('|')
device_id = msg_split[1].data().decode('ascii')
device_version = msg_split[2].data().decode('ascii')
now = datetime.datetime.now()
ip = QHostAddress(msg[1].toIPv4Address()).toString()
device = {"id": device_id, "version": device_version, "ip": ip, "last_seen": now}
self.nodeListMutex.lock()
if (device_id not in self.connectedNodes.keys()) and \
(device_id not in self.visibleNodes.keys()):
self.visibleNodes[device_id] = device
self.newNodeDiscovered.emit(device)
print(f"discovered {device_id}")
# update timestamps for known visible/connected devices
if device_id in self.visibleNodes.keys():
self.visibleNodes[device_id]["last_seen"] = now
self.nodeAliveMessage.emit(self.visibleNodes[device_id])
if device_id in self.connectedNodes.keys():
self.connectedNodes[device_id]["last_seen"] = now
self.nodeAliveMessage.emit(self.connectedNodes[device_id])
self.nodeListMutex.unlock()
@Slot(str)
def onSocketCanDiscovered(self, vcan):
now = datetime.datetime.now()
device = {"id": vcan, "version": "socket_can", "ip": None, "last_seen": now}
self.nodeListMutex.lock()
if (vcan not in self.connectedNodes.keys()) and \
(vcan not in self.visibleNodes.keys()):
self.visibleNodes[vcan] = device
self.newNodeDiscovered.emit(device)
if vcan in self.visibleNodes.keys():
self.visibleNodes[vcan]["last_seen"] = now
self.nodeAliveMessage.emit(self.visibleNodes[vcan])
if vcan in self.connectedNodes.keys():
self.connectedNodes[vcan]["last_seen"] = now
self.nodeAliveMessage.emit(self.connectedNodes[vcan])
self.nodeListMutex.unlock()
@Slot()
def onDisconnectTimerFire(self):
now = datetime.datetime.now()
self.nodeListMutex.lock()
ids_to_delete = []
for id, node in self.visibleNodes.items():
# check time difference
if (now - node["last_seen"]) > datetime.timedelta(seconds=6):
ids_to_delete.append(id)
self.nodeDisappeared.emit(node)
for id in ids_to_delete:
del self.visibleNodes[id]
ids_to_delete = []
for id, node in self.connectedNodes.items():
# check time difference
if (now - node["last_seen"]) > datetime.timedelta(seconds=10):
ids_to_delete.append(id)
self.nodeDisconnected.emit(node)
for id in ids_to_delete:
# check for running connection process
if self.connectedNodes[id]["connection"].isConnected:
self.connectedNodes[id]["connection"].resetConnection()
del self.connectedNodes[id]
self.nodeListMutex.unlock()
@Slot(dict)
def onConnectToNode(self, node):
print(f"Node handler creating connection to {node['id']}.")
if ("connection" not in node.keys() or node["connection"] is None) and node["ip"] is not None:
# start a connection to a canbadger
con = NodeConnection(node)
node["connection"] = con
con.connectionSucceeded.connect(self.onConnectionSucceeded)
con.connectionFailed.connect(self.onConnectionFailed)
con.newSettingsData.connect(self.mainwindow.onSettingsReceived)
self.mainwindow.exiting.connect(con.resetConnection)
con.onRun()
elif ("connection" not in node.keys() or node["connection"] is None) and node["ip"] is None:
# start a socketCan connection
con = SocketCanConnection(node)
node["connection"] = con
con.connectionSucceeded.connect(self.onConnectionSucceeded)
con.connectionFailed.connect(self.onConnectionFailed)
self.mainwindow.exiting.connect(con.cleanup)
con.onRun()
@Slot()
def onConnectionSucceeded(self):
node_connection = self.sender()
node = node_connection.node
self.nodeListMutex.lock()
if node["id"] in self.visibleNodes.keys():
del self.visibleNodes[node["id"]]
self.connectedNodes[node["id"]] = node
self.nodeListMutex.unlock()
self.nodeConnected.emit(node)
node_connection.nodeDisconnected.connect(self.onDisconnectNode)
@Slot()
def onConnectionFailed(self):
# signal red
buttonFeedback(False, self.mainwindow.interfaceSelection)
@Slot(dict)
def onDisconnectNode(self, node):
# reset connection and terminate thread
if "connection" in node.keys():
node["connection"].resetConnection()
if "thread" in node.keys():
node["thread"].exit()
pass
# delete the node from dicts
self.nodeListMutex.lock()
if node["id"] in self.connectedNodes:
del self.connectedNodes[node["id"]]
if node["id"] in self.visibleNodes:
del self.visibleNodes[node["id"]]
self.nodeListMutex.unlock()
self.nodeDisconnected.emit(node)
@Slot(str, str)
def onIDChange(self, old, new):
self.nodeListMutex.lock()
if old in self.connectedNodes:
self.connectedNodes[new] = self.connectedNodes[old]
del self.connectedNodes[old]
if old in self.visibleNodes:
self.visibleNodes[new] = self.visibleNodes[old]
del self.visibleNodes[old]
self.nodeListMutex.unlock()
@Slot()
def onExiting(self):
self.disconnectTimer.stop()
self.udpSocket.close()
self.cleanupDone.emit()