def run(self):
self.mutex.lock()
serverName = self.hostName
serverPort = self.port
self.mutex.unlock()
while not self.quit:
Timeout = 5 * 1000
socket = QTcpSocket()
socket.connectToHost(serverName, serverPort)
if not socket.waitForConnected(Timeout):
self.error.emit(socket.error(), socket.errorString())
return
while socket.bytesAvailable() < 2:
if not socket.waitForReadyRead(Timeout):
self.error.emit(socket.error(), socket.errorString())
return
instr = QDataStream(socket)
instr.setVersion(QDataStream.Qt_4_0)
blockSize = instr.readUInt16()
block = QByteArray()
outstr = QDataStream(block, QIODevice.WriteOnly)
outstr.setVersion(QDataStream.Qt_4_0)
while socket.bytesAvailable() < blockSize:
if not socket.waitForReadyRead(Timeout):
self.error.emit(socket.error(), socket.errorString())
return
self.mutex.lock()
outstr.writeUInt16(0)
outstr.writeQString("Message to Server")
outstr.device().seek(0)
outstr.writeUInt16(block.size() - 2)
socket.write(block)
# socket.write(block)
fortune = instr.readQString()
self.newFortune.emit(fortune)
self.cond.wait(self.mutex)
serverName = self.hostName
serverPort = self.port
self.mutex.unlock()
def sendClientCommand(host, port, cmd, wait_time=WAIT_TIME_MS):
# Try to connect to a host server
tcpSocket = QTcpSocket()
tcpSocket.connectToHost(host, port, QIODevice.ReadWrite)
if not tcpSocket.waitForConnected(msecs=wait_time):
return CLIENT_ERROR_NO_CONNECTION
# Prepare a command message to be sent
block = QByteArray()
outstr = QDataStream(block, QIODevice.WriteOnly)
outstr.setVersion(QDataStream.Qt_4_0)
outstr.writeUInt16(0)
outstr.writeQString(cmd)
outstr.device().seek(0)
outstr.writeUInt16(block.size() - 2)
tcpSocket.write(block)
# Try to send the message
if not tcpSocket.waitForBytesWritten(msecs=wait_time):
return CLIENT_ERROR_BLOCK_NOT_WRITTEN
# Wait for a response from the host server
if not tcpSocket.waitForReadyRead(msecs=10000):
return CLIENT_ERROR_NO_RESPONSE
# Try to read the response
instr = QDataStream(tcpSocket)
instr.setVersion(QDataStream.Qt_4_0)
blockSize = 0
if blockSize == 0:
if tcpSocket.bytesAvailable() < 2:
return CLIENT_ERROR_RESPONSE_NOT_COMPLETE
blockSize = instr.readUInt16()
if tcpSocket.bytesAvailable() < blockSize:
return CLIENT_ERROR_RESPONSE_NOT_COMPLETE
# Wait until the host server terminates the connection
tcpSocket.waitForDisconnected()
# Return value representing a command execution status
if instr.readString() == COMMAND_EXECUTED_CONFIRMATION_MESSAGE:
return CLIENT_COMMAND_EXECUTED
else:
return CLIENT_COMMAND_FAILED
def run(self):
self.mutex.lock()
serverName = self.hostName
serverPort = self.port
self.mutex.unlock()
while not self.quit:
Timeout = 5 * 1000
socket = QTcpSocket()
socket.connectToHost(serverName, serverPort)
if not socket.waitForConnected(Timeout):
self.error.emit(socket.error(), socket.errorString())
return
while socket.bytesAvailable() < 2:
if not socket.waitForReadyRead(Timeout):
self.error.emit(socket.error(), socket.errorString())
return
instr = QDataStream(socket)
instr.setVersion(QDataStream.Qt_4_0)
blockSize = instr.readUInt16()
while socket.bytesAvailable() < blockSize:
if not socket.waitForReadyRead(Timeout):
self.error.emit(socket.error(), socket.errorString())
return
self.mutex.lock()
fortune = instr.readQString()
self.newFortune.emit(fortune)
self.cond.wait(self.mutex)
serverName = self.hostName
serverPort = self.port
self.mutex.unlock()
def run(self):
self.mutex.lock()
serverName = self.hostName
serverPort = self.port
self.mutex.unlock()
while not self.quit:
Timeout = 5 * 1000
socket = QTcpSocket()
socket.connectToHost(serverName, serverPort)
if not socket.waitForConnected(Timeout):
self.error.emit(socket.error(), socket.errorString())
return
while socket.bytesAvailable() < 2:
if not socket.waitForReadyRead(Timeout):
self.error.emit(socket.error(), socket.errorString())
return
instr = QDataStream(socket)
instr.setVersion(QDataStream.Qt_4_0)
blockSize = instr.readUInt16()
while socket.bytesAvailable() < blockSize:
if not socket.waitForReadyRead(Timeout):
self.error.emit(socket.error(), socket.errorString())
return
self.mutex.lock()
fortune = instr.readQString()
self.newFortune.emit(fortune)
self.cond.wait(self.mutex)
serverName = self.hostName
serverPort = self.port
self.mutex.unlock()
class DSKY(QMainWindow):
def __init__(self, parent):
super().__init__(parent)
self.setWindowTitle('mDSKY')
try:
self._el_serial = serial.Serial('/dev/dsky_el', 57600)
self._el_serial.write(b'R\nR\n')
except:
self._el_serial = None
try:
self._il_serial = serial.Serial('/dev/dsky_il', 57600)
except:
self._il_serial = None
self._setup_ui()
self._packet = [0, 0, 0, 0]
self._packet_idx = 0
self._socket = QTcpSocket(self)
self._socket.readyRead.connect(self._read_data)
self._socket.disconnected.connect(self._connect_to_magc)
self._connect_to_magc()
self._last_el_cmd = b''
self._last_il_cmd = b''
self._timer = QTimer()
self._timer.timeout.connect(self._update_display)
self._timer.start(10)
def _connect_to_magc(self):
self._socket.connectToHost('localhost', 19681)
connected = self._socket.waitForConnected(RECONNECT_MS)
if not connected:
QTimer.singleShot(RECONNECT_MS, self._connect_to_magc)
def _read_data(self):
data = self._socket.read(STATE_SIZE * 10)
while data:
(out0, vnflash, restart, oper_err, key_rel, temp, upl_act,
comp_act, stby) = struct.unpack_from(STATE_FMT, data, 0)
data = data[STATE_SIZE:]
self._update_segments(out0)
self._verb[0].set_on(not vnflash)
self._verb[1].set_on(not vnflash)
self._noun[0].set_on(not vnflash)
self._noun[1].set_on(not vnflash)
self._key_rel.set_on(key_rel)
self._opr_err.set_on(oper_err)
self._upl_act.set_on(upl_act)
self._com_act.set_on(comp_act)
self._restart.set_on(restart)
self._temp.set_on(temp)
self._stby.set_on(stby)
def _update_segments(self, out0):
relay = (out0 >> 11) & 0x0F
relay_value = out0 & 0o3777
if relay == 1:
self._sign3.set_minus_bit((relay_value >> 10) & 0o1)
self._reg3[3].set_relay_bits((relay_value >> 5) & 0o37)
self._reg3[4].set_relay_bits((relay_value >> 0) & 0o37)
elif relay == 2:
self._sign3.set_plus_bit((relay_value >> 10) & 0o1)
self._reg3[1].set_relay_bits((relay_value >> 5) & 0o37)
self._reg3[2].set_relay_bits((relay_value >> 0) & 0o37)
elif relay == 3:
self._reg2[4].set_relay_bits((relay_value >> 5) & 0o37)
self._reg3[0].set_relay_bits((relay_value >> 0) & 0o37)
elif relay == 4:
self._sign2.set_minus_bit((relay_value >> 10) & 0o1)
self._reg2[2].set_relay_bits((relay_value >> 5) & 0o37)
self._reg2[3].set_relay_bits((relay_value >> 0) & 0o37)
elif relay == 5:
self._sign2.set_plus_bit((relay_value >> 10) & 0o1)
self._reg2[0].set_relay_bits((relay_value >> 5) & 0o37)
self._reg2[1].set_relay_bits((relay_value >> 0) & 0o37)
elif relay == 6:
self._sign1.set_minus_bit((relay_value >> 10) & 0o1)
self._reg1[3].set_relay_bits((relay_value >> 5) & 0o37)
self._reg1[4].set_relay_bits((relay_value >> 0) & 0o37)
elif relay == 7:
self._sign1.set_plus_bit((relay_value >> 10) & 0o1)
self._reg1[1].set_relay_bits((relay_value >> 5) & 0o37)
self._reg1[2].set_relay_bits((relay_value >> 0) & 0o37)
elif relay == 8:
self._reg1[0].set_relay_bits((relay_value >> 0) & 0o37)
elif relay == 9:
self._noun[0].set_relay_bits((relay_value >> 5) & 0o37)
self._noun[1].set_relay_bits((relay_value >> 0) & 0o37)
elif relay == 10:
self._verb[0].set_relay_bits((relay_value >> 5) & 0o37)
self._verb[1].set_relay_bits((relay_value >> 0) & 0o37)
elif relay == 11:
self._prog[0].set_relay_bits((relay_value >> 5) & 0o37)
self._prog[1].set_relay_bits((relay_value >> 0) & 0o37)
elif relay == 12:
self._prio_disp.set_on((relay_value >> 0) & 0o1)
self._no_dap.set_on((relay_value >> 1) & 0o1)
self._vel.set_on((relay_value >> 2) & 0o1)
self._no_att.set_on((relay_value >> 3) & 0o1)
self._alt.set_on((relay_value >> 4) & 0o1)
self._gimbal_lock.set_on((relay_value >> 5) & 0o1)
self._tracker.set_on((relay_value >> 7) & 0o1)
self._prog_alarm.set_on((relay_value >> 8) & 0o1)
def _setup_ui(self):
self.setObjectName('#DSKY')
self.setWindowFlags(Qt.Window)
self.setFixedSize(500, 580)
self.setStyleSheet(
'DSKY{background-image: url(:/resources/dsky.png);}')
self.setWindowTitle('mDSKY')
el_pix = QPixmap(':/resources/el.png')
lamp_pix = QPixmap(':/resources/lamps.png')
self._com_act = Lamp(self, el_pix, 0, 1, 65, 61, False)
self._com_act.move(285, 36)
self._perms = self._create_permanent_segments(el_pix)
self._prog = self._create_mode(el_pix, 394, 60)
self._verb = self._create_mode(el_pix, 287, 129)
self._noun = self._create_mode(el_pix, 394, 129)
self._sign1, self._reg1 = self._create_reg(el_pix, 287, 184)
self._sign2, self._reg2 = self._create_reg(el_pix, 287, 239)
self._sign3, self._reg3 = self._create_reg(el_pix, 287, 294)
self._but_verb = self._create_button(8, 404, 0b10001) # VERB
self._but_noun = self._create_button(8, 474, 0b11111) # NOUN
self._but_plus = self._create_button(78, 369, 0b11010) # +
self._but_minus = self._create_button(78, 439, 0b11011) # -
self._but_0 = self._create_button(78, 509, 0b10000) # 0
self._but_7 = self._create_button(148, 369, 0b00111) # 7
self._but_4 = self._create_button(148, 439, 0b00100) # 4
self._but_1 = self._create_button(148, 509, 0b00001) # 1
self._but_8 = self._create_button(218, 369, 0b01000) # 8
self._but_5 = self._create_button(218, 439, 0b00101) # 5
self._but_2 = self._create_button(218, 509, 0b00010) # 2
self._but_9 = self._create_button(288, 369, 0b01001) # 9
self._but_6 = self._create_button(288, 439, 0b00110) # 6
self._but_3 = self._create_button(288, 509, 0b00011) # 3
self._but_clr = self._create_button(359, 369, 0b11110) # CLR
self._but_pro = self._create_button(359, 439, 0b100000) # PRO
self._but_key_rel = self._create_button(359, 509, 0b11001) # KEY REL
self._but_enter = self._create_button(429, 404, 0b11100) # ENTER
self._but_reset = self._create_button(429, 474, 0b10010) # RESET
self._upl_act = Lamp(self, lamp_pix, 0, 0, 78, 37, False)
self._upl_act.move(45, 34)
self._no_att = Lamp(self, lamp_pix, 0, 38, 78, 37, False)
self._no_att.move(45, 77)
self._stby = Lamp(self, lamp_pix, 0, 76, 78, 37, False)
self._stby.move(45, 120)
self._key_rel = Lamp(self, lamp_pix, 0, 114, 78, 37, False)
self._key_rel.move(45, 163)
self._opr_err = Lamp(self, lamp_pix, 0, 152, 78, 37, False)
self._opr_err.move(45, 206)
self._prio_disp = Lamp(self, lamp_pix, 0, 190, 78, 37, False)
self._prio_disp.move(45, 249)
self._no_dap = Lamp(self, lamp_pix, 0, 228, 78, 37, False)
self._no_dap.move(45, 292)
self._temp = Lamp(self, lamp_pix, 79, 0, 78, 37, False)
self._temp.move(134, 34)
self._gimbal_lock = Lamp(self, lamp_pix, 79, 38, 78, 37, False)
self._gimbal_lock.move(134, 77)
self._prog_alarm = Lamp(self, lamp_pix, 79, 76, 78, 37, False)
self._prog_alarm.move(134, 120)
self._restart = Lamp(self, lamp_pix, 79, 114, 78, 37, False)
self._restart.move(134, 163)
self._tracker = Lamp(self, lamp_pix, 79, 152, 78, 37, False)
self._tracker.move(134, 206)
self._alt = Lamp(self, lamp_pix, 79, 190, 78, 37, False)
self._alt.move(134, 249)
self._vel = Lamp(self, lamp_pix, 79, 228, 78, 37, False)
self._vel.move(134, 292)
self._il_lamps = [
self._upl_act, self._no_att, self._stby, self._key_rel,
self._opr_err, self._prio_disp, self._no_dap, self._temp,
self._gimbal_lock, self._prog_alarm, self._restart, self._tracker,
self._alt, self._vel
]
self.show()
def _create_reg(self, el_pix, col, row):
digits = []
sign = Sign(self, el_pix)
sign.move(col, row + 6)
for i in range(5):
ss = SevenSegment(self, el_pix)
ss.move(col + 18 + 30 * i, row)
digits.append(ss)
return sign, digits
def _create_mode(self, el_pix, col, row):
digits = []
for i in range(2):
ss = SevenSegment(self, el_pix)
ss.move(col + 30 * i, row)
digits.append(ss)
return digits
def _create_permanent_segments(self, el_pix):
perms = []
# PROG
el = Lamp(self, el_pix, 66, 0, 64, 19, True)
el.move(393, 37)
perms.append(el)
# VERB
el = Lamp(self, el_pix, 66, 41, 64, 20, True)
el.move(286, 106)
perms.append(el)
# NOUN
el = Lamp(self, el_pix, 66, 20, 64, 20, True)
el.move(393, 106)
perms.append(el)
for i in range(3):
el = Lamp(self, el_pix, 0, 63, 142, 5, True)
el.move(305, 170 + i * 55)
perms.append(el)
def _create_button(self, x, y, keycode):
b = Button(self)
b.setFixedSize(63, 63)
b.move(x, y)
b.setStyleSheet('QPushButton{background-color: rgba(0,0,0,0);}')
b.setFocusPolicy(Qt.FocusPolicy.NoFocus)
b.setAutoRepeat(False)
if keycode == 0b100000:
key_on = keycode | 0b1
key_off = keycode
else:
key_on = keycode
key_off = 0
b.pressed.connect(lambda: self._send_key(key_on))
b.released.connect(lambda: self._send_key(key_off))
return b
def _send_key(self, k):
self._socket.write(QByteArray(struct.pack('<B', k)))
def _update_display(self):
reg1 = b''.join([ss.value for ss in self._reg1])
reg2 = b''.join([ss.value for ss in self._reg2])
reg3 = b''.join([ss.value for ss in self._reg3])
verb = b''.join([ss.value for ss in self._verb])
noun = b''.join([ss.value for ss in self._noun])
prog = b''.join([ss.value for ss in self._prog])
el_cmd = b'DA%s%s%s%s%s%s%s%s%s%s\n' % (
self._sign1.value, reg1, self._sign2.value, reg2,
self._sign3.value, reg3, verb, noun, self._com_act.value, prog)
if el_cmd != self._last_el_cmd:
if self._el_serial is not None:
self._el_serial.write(el_cmd)
self._el_serial.readline()
self._last_el_cmd = el_cmd
il_values = 0
for i in range(len(self._il_lamps)):
if self._il_lamps[i].value == b'1':
il_values |= 1 << i
il_cmd = b'M%04X\n' % il_values
if il_cmd != self._last_il_cmd:
if self._il_serial is not None:
self._il_serial.write(il_cmd)
self._il_serial.readline()
self._last_il_cmd = il_cmd
def paintEvent(self, event):
opt = QStyleOption()
opt.init(self)
p = QPainter(self)
self.style().drawPrimitive(QStyle.PE_Widget, opt, p, self)
def keyPressEvent(self, event):
if event.isAutoRepeat():
return
key = event.key()
self._set_key_down(key, True)
def keyReleaseEvent(self, event):
if event.isAutoRepeat():
return
key = event.key()
self._set_key_down(key, False)
def _set_key_down(self, key, down):
but = None
if key == Qt.Key.Key_V:
but = self._but_verb
elif key == Qt.Key.Key_N:
but = self._but_noun
elif key == Qt.Key.Key_V:
but = self._but_verb
elif key == Qt.Key.Key_Plus or key == Qt.Key.Key_Equal:
but = self._but_plus
elif key == Qt.Key.Key_Minus:
but = self._but_minus
elif key == Qt.Key.Key_0:
but = self._but_0
elif key == Qt.Key.Key_1:
but = self._but_1
elif key == Qt.Key.Key_2:
but = self._but_2
elif key == Qt.Key.Key_3:
but = self._but_3
elif key == Qt.Key.Key_4:
but = self._but_4
elif key == Qt.Key.Key_5:
but = self._but_5
elif key == Qt.Key.Key_6:
but = self._but_6
elif key == Qt.Key.Key_7:
but = self._but_7
elif key == Qt.Key.Key_8:
but = self._but_8
elif key == Qt.Key.Key_9:
but = self._but_9
elif key == Qt.Key.Key_C:
but = self._but_clr
elif key == Qt.Key.Key_P:
but = self._but_pro
elif key == Qt.Key.Key_K:
but = self._but_key_rel
elif key == Qt.Key.Key_E:
but = self._but_enter
elif key == Qt.Key.Key_R:
but = self._but_reset
if but is None:
return
if down:
but.press()
else:
but.release()
class CommandClient:
## @property host
# A QtHostAddress to the `CommandServer`.
## @property port
# An int of port at which `CommandServer` is listening.
## @property tcpSocket
# A QTcpSocket used to contact `CommandSErver`
## @property blockSize
# An int representing size of incomming tcp message.
## @brief Initialization method for CommandClient.
#
#A class instance is created and its attributes are initialized.
#
#
# @param host A QtHostAddress to the `CommandServer`.
# @param port An int of port at which `CommandServer` is listening.
#
def __init__(self, host, port):
self.host = host
self.port = port
self.tcpSocket = QTcpSocket()
self.blockSize = 0
## @brief Method used to send commands from client to `CommandServer`.
#
#This method tries to connect to a specified host `CommandServer` via
#`tcpSocket`. If connection was successfull, the command `cmd` is sent.
#Then the response is expected. If the response is equal to
#COMMAND_EXECUTED_CONFIRMATION_MESSAGE, then the execution was successfull.
#The progress and result of `sendCommand` can be obtained from printed logs and
#return value.
#
#
# @param cmd A str command to be executed.
#
# @return
# `CLIENT_COMMAND_EXECUTED` if all went great and command was executed.
# `CLIENT_COMMAND_FAILED` if `cmd` execution failed.
# `CLIENT_ERROR_RESPONSE_NOT_COMPLETE` if a response received was incomplete.
# `CLIENT_ERROR_NO_RESPONSE` if there was no response within `WAIT_TIME_MS`.
# `CLIENT_ERROR_BLOCK_NOT_WRITTEN` if communication failed during sending.
# `CLIENT_ERROR_NO_CONNECTION` if no connection to a host was established.
#
def sendCommand(self, cmd):
# connect a Qt slot to receive and print errors
self.tcpSocket.error.connect(self.displayError)
# Try to connect to a host server
self.tcpSocket.connectToHost(self.host, self.port, QIODevice.ReadWrite)
if not self.tcpSocket.waitForConnected(msecs=WAIT_TIME_MS):
if "FreeCAD" in sys.modules:
FreeCAD.Console.PrintError(
"CommandClient.sendCommand error: " + "No connection\n")
else:
print("CommandClient.sendCommand error: No connection\n")
return CLIENT_ERROR_NO_CONNECTION
# Prepare a command message to be sent
block = QByteArray()
outstr = QDataStream(block, QIODevice.WriteOnly)
outstr.setVersion(QDataStream.Qt_4_0)
outstr.writeUInt16(0)
outstr.writeQString(cmd)
outstr.device().seek(0)
outstr.writeUInt16(block.size() - 2)
# Try to send the message
if "FreeCAD" in sys.modules:
FreeCAD.Console.PrintMessage("CommandClient sending> " + cmd +
"\n")
else:
print("CommandClient sending> " + cmd + "\n")
self.tcpSocket.write(block)
if not self.tcpSocket.waitForBytesWritten(msecs=WAIT_TIME_MS):
if "FreeCAD" in sys.modules:
FreeCAD.Console.PrintError(
"CommandClient.sendCommand error: " +
"Block not written\n")
else:
print("CommandClient.sendCommand error: Block not written\n")
return CLIENT_ERROR_BLOCK_NOT_WRITTEN
# Wait for a response from the host server
if not self.tcpSocket.waitForReadyRead(msecs=WAIT_TIME_MS):
if "FreeCAD" in sys.modules:
FreeCAD.Console.PrintError(
"CommandClient.sendCommand error: " +
"No response received.\n")
else:
print("CommandClient.sendCommand error: " +
"No response received.\n")
return CLIENT_ERROR_NO_RESPONSE
# Try to read the response
instr = QDataStream(self.tcpSocket)
instr.setVersion(QDataStream.Qt_4_0)
if self.blockSize == 0:
if self.tcpSocket.bytesAvailable() < 2:
return CLIENT_ERROR_RESPONSE_NOT_COMPLETE
self.blockSize = instr.readUInt16()
if self.tcpSocket.bytesAvailable() < self.blockSize:
return CLIENT_ERROR_RESPONSE_NOT_COMPLETE
response = instr.readString()
if "FreeCAD" in sys.modules:
FreeCAD.Console.PrintMessage("CommandClient received> " +
response + "\n")
else:
print("CommandClient received> " + response + "\n")
# Wait until the host server terminates the connection
self.tcpSocket.waitForDisconnected()
# Reset blockSize to prepare for sending next command
self.blockSize = 0
# Return value representing a command execution status
if response == COMMAND_EXECUTED_CONFIRMATION_MESSAGE:
return CLIENT_COMMAND_EXECUTED
else:
return CLIENT_COMMAND_FAILED
## @brief `Qt`'s slot method to print out received `tcpSocket`'s error.
#
#QAbstractSocket.RemoteHostClosedError is not printed, because it occurs
#naturaly when the `tcpSocket` closes after a transaction is over. Except that
#all errors are printed.
#
#
# @param socketError A QAbstractSocket::SocketError enum describing occured error.
#
def displayError(self, socketError):
if socketError != QAbstractSocket.RemoteHostClosedError:
if "FreeCAD" in sys.modules:
FreeCAD.Console.PrintError(
"CommandClient error occurred> %s." %
self.tcpSocket.errorString() + "\n")
else:
print("CommandClient error occurred> %s." %
self.tcpSocket.errorString() + "\n")
class MantaSocket:
def __init__(self) -> None:
self.socket = QTcpSocket()
self.timer = QTimer()
self.timer.timeout.connect(self.client_listening)
self.timer.start(1000)
self.socket.readyRead.connect(self.read_data)
self.socket.disconnected.connect(self.client_socket_disconnected)
self.writeflag = 0
def client_listening(self):
if self.socket.state() == QAbstractSocket.ConnectedState:
pass
else:
self.socket.connectToHost('192.168.7.2', 2345)
if not self.socket.waitForConnected(200):
return
print('Connected!!!')
# self.write_data()
def read_data(self):
print("read!!!")
receive_data = self.socket.readAll()
if not receive_data.isEmpty():
# print(socketdata)
self.socket_data_analysis(receive_data)
def write_data(self):
# obj=bytes('hello'.encode())
# send_str=QByteArray(obj)
send_str = 'hello yeah world'
if self.writeflag:
self.socket.writeData(send_str, len(send_str))
def client_socket_disconnected(self):
print('Socket Disconnect!!!')
def socket_data_analysis(self, receive):
'''
pos_left,pos_right,pos_left_roll,pos_right_roll,dpos_left,dpos_right,dpos_left_roll,dpos_right_roll,pos_behind_left,pos_behind_right,
roll,yaw,left_A,right_A,yaw_desire,pitch,left_behind,right_behind,pitch_desire,depth_c,depth;
'''
# print(receive)
pos_str = "$POS"
pos_str_index = receive.indexOf(pos_str.encode())
aux_num = 0
data_index = 0
data_index = pos_str_index + (4 + 1)
aux_num = 5
pos_left = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 5
pos_right = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 5
pos_left_roll = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 5
pos_right_roll = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 5
dpos_left = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 5
dpos_right = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 5
dpos_left_roll = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 5
dpos_right_roll = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 5
pos_behind_left = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 5
pos_behind_right = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 6
roll = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 6
yaw = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 6
left_A = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 6
right_A = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 6
yaw_desire = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 6
pitch = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 6
left_behind = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 6
right_behind = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 6
pitch_desire = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 6
depth_c = receive.mid(data_index, aux_num).toFloat()[0]
data_index += aux_num + 1
aux_num = 7
depth = receive.mid(data_index, aux_num).toFloat()[0]
# Window UI Display
window.ui.pitch_label.setText(str(pitch))
window.ui.roll_label.setText(str(roll))
window.ui.yaw_label.setText(str(yaw))
window.ui.depth_label.setText(str(depth))
def sendClientCommand(host, port, cmd, wait_time=WAIT_TIME_MS):
"""
Method to be used for sending commands.
This method is an alternative to using `CommandClient`. It does not print any
logs, just returns a value saying how the execution went.
To send a command using this method do:
sendClientCommand("127.0.0.1",54333,
"FreeCAD.Console.PrintWarning('Hello World\\n')")
Args:
cmd: A str command to be executed.
host: A QtHostAddress to the `CommandServer`.
port: An int of port at which `CommandServer` is listening.
Kwargs:
wait_time: An int setting milliseconds to wait for connection or message.
Returns:
`CLIENT_COMMAND_EXECUTED` if all went great and command was executed.
`CLIENT_COMMAND_FAILED` if `cmd` execution failed.
`CLIENT_ERROR_RESPONSE_NOT_COMPLETE` if a response received was incomplete.
`CLIENT_ERROR_NO_RESPONSE` if there was no response within `WAIT_TIME_MS`.
`CLIENT_ERROR_BLOCK_NOT_WRITTEN` if communication failed during sending.
`CLIENT_ERROR_NO_CONNECTION` if no connection to a host was established.
"""
# Try to connect to a host server
tcpSocket = QTcpSocket()
tcpSocket.connectToHost(host, port, QIODevice.ReadWrite)
if not tcpSocket.waitForConnected(msecs=wait_time):
return CLIENT_ERROR_NO_CONNECTION
# Prepare a command message to be sent
block = QByteArray(
len(cmd.encode("UTF-8")).to_bytes(2, byteorder='big') +
cmd.encode("UTF-8"))
outstr = QDataStream(block, QIODevice.WriteOnly)
outstr.setVersion(QDataStream.Qt_4_0)
tcpSocket.write(block)
# Try to send the message
if not tcpSocket.waitForBytesWritten(msecs=wait_time):
return CLIENT_ERROR_BLOCK_NOT_WRITTEN
# Wait for a response from the host server
if not tcpSocket.waitForReadyRead(msecs=wait_time):
return CLIENT_ERROR_NO_RESPONSE
# Try to read the response
instr = QDataStream(tcpSocket)
instr.setVersion(QDataStream.Qt_4_0)
blockSize = 0
if blockSize == 0:
if tcpSocket.bytesAvailable() < 2:
return CLIENT_ERROR_RESPONSE_NOT_COMPLETE
blockSize = instr.readUInt16()
if tcpSocket.bytesAvailable() < blockSize:
return CLIENT_ERROR_RESPONSE_NOT_COMPLETE
# Wait until the host server terminates the connection
tcpSocket.waitForDisconnected()
# Return value representing a command execution status
if instr.readRawData(blockSize).decode("UTF-8") \
== COMMAND_EXECUTED_CONFIRMATION_MESSAGE:
return CLIENT_COMMAND_EXECUTED
else:
return CLIENT_COMMAND_FAILED
class CommandClient:
"""
Class to be used for sending commands.
This class can be used in FreeCAD's or regular python console to send commands
to a `CommandServer` using `sendCommand()`. The class prints logs as it moves
along.
Attributes:
host: A QtHostAddress to the `CommandServer`.
port: An int of port at which `CommandServer` is listening.
tcpSocket: A QTcpSocket used to contact `CommandSErver`
blockSize: An int representing size of incoming tcp message.
To send a commands do:
client = CommandClient("127.0.0.1",54321)
client.sendCommand('FreeCAD.Console.PrintError("Hello World\\n")\n')
client.sendCommand('FreeCAD.Console.PrintError("Bye Bye\\n")\n')
"""
def __init__(self, host, port):
"""
Initialization method for CommandClient.
A class instance is created and its attributes are initialized.
Args:
host: A QtHostAddress to the `CommandServer`.
port: An int of port at which `CommandServer` is listening.
"""
self.host = host
self.port = port
self.tcpSocket = QTcpSocket()
self.blockSize = 0
def sendCommand(self, cmd):
"""
Method used to send commands from client to `CommandServer`.
This method tries to connect to a specified host `CommandServer` via
`tcpSocket`. If connection was successful, the command `cmd` is sent.
Then the response is expected. If the response is equal to
COMMAND_EXECUTED_CONFIRMATION_MESSAGE, then the execution was successful.
The progress and result of `sendCommand` can be obtained from printed logs and
return value.
Args:
cmd: A str command to be executed.
Returns:
`CLIENT_COMMAND_EXECUTED` if all went great and command was executed.
`CLIENT_COMMAND_FAILED` if `cmd` execution failed.
`CLIENT_ERROR_RESPONSE_NOT_COMPLETE` if a response received was incomplete.
`CLIENT_ERROR_NO_RESPONSE` if there was no response within `WAIT_TIME_MS`.
`CLIENT_ERROR_BLOCK_NOT_WRITTEN` if communication failed during sending.
`CLIENT_ERROR_NO_CONNECTION` if no connection to a host was established.
"""
# connect a Qt slot to receive and print errors
self.tcpSocket.error.connect(self.displayError)
# Try to connect to a host server
self.tcpSocket.connectToHost(self.host, self.port, QIODevice.ReadWrite)
if not self.tcpSocket.waitForConnected(msecs=WAIT_TIME_MS):
if "FreeCAD" in sys.modules:
FreeCAD.Console.PrintError(
"CommandClient.sendCommand error: " + "No connection\n")
else:
print("CommandClient.sendCommand error: No connection\n")
return CLIENT_ERROR_NO_CONNECTION
# Prepare a command message to be sent
block = QByteArray(
len(cmd.encode("UTF-8")).to_bytes(2, byteorder='big') +
cmd.encode("UTF-8"))
outstr = QDataStream(block, QIODevice.WriteOnly)
outstr.setVersion(QDataStream.Qt_4_0)
# Try to send the message
if "FreeCAD" in sys.modules:
FreeCAD.Console.PrintMessage("CommandClient sending> " + cmd +
"\n")
else:
print("CommandClient sending> " + cmd + "\n")
self.tcpSocket.write(block)
if not self.tcpSocket.waitForBytesWritten(msecs=WAIT_TIME_MS):
if "FreeCAD" in sys.modules:
FreeCAD.Console.PrintError(
"CommandClient.sendCommand error: " +
"Block not written\n")
else:
print("CommandClient.sendCommand error: Block not written\n")
return CLIENT_ERROR_BLOCK_NOT_WRITTEN
# Wait for a response from the host server
if not self.tcpSocket.waitForReadyRead(msecs=WAIT_TIME_MS):
if "FreeCAD" in sys.modules:
FreeCAD.Console.PrintError(
"CommandClient.sendCommand error: " +
"No response received.\n")
else:
print("CommandClient.sendCommand error: " +
"No response received.\n")
return CLIENT_ERROR_NO_RESPONSE
# Try to read the response
instr = QDataStream(self.tcpSocket)
instr.setVersion(QDataStream.Qt_4_0)
if self.blockSize == 0:
if self.tcpSocket.bytesAvailable() < 2:
return CLIENT_ERROR_RESPONSE_NOT_COMPLETE
self.blockSize = instr.readUInt16()
if self.tcpSocket.bytesAvailable() < self.blockSize:
return CLIENT_ERROR_RESPONSE_NOT_COMPLETE
response = instr.readRawData(self.blockSize).decode("UTF-8")
if "FreeCAD" in sys.modules:
FreeCAD.Console.PrintMessage("CommandClient received> " +
response + "\n")
else:
print("CommandClient received> " + response + "\n")
# Wait until the host server terminates the connection
self.tcpSocket.waitForDisconnected()
# Reset blockSize to prepare for sending next command
self.blockSize = 0
# Return value representing a command execution status
if response == COMMAND_EXECUTED_CONFIRMATION_MESSAGE:
return CLIENT_COMMAND_EXECUTED
else:
return CLIENT_COMMAND_FAILED
def displayError(self, socketError):
"""
`Qt`'s slot method to print out received `tcpSocket`'s error.
QAbstractSocket.RemoteHostClosedError is not printed, because it occurs
naturally when the `tcpSocket` closes after a transaction is over. Except that
all errors are printed.
Args:
socketError: A QAbstractSocket::SocketError enum describing occurred error.
"""
if socketError != QAbstractSocket.RemoteHostClosedError:
if "FreeCAD" in sys.modules:
FreeCAD.Console.PrintError(
"CommandClient error occurred> %s." %
self.tcpSocket.errorString() + "\n")
else:
print("CommandClient error occurred> %s." %
self.tcpSocket.errorString() + "\n")
