def tcpClientTest():
ipAddress = "192.168.1.107"
port = 6000
client = TcpClient(ipAddress, port)
print("Trying to connect...")
client.connect()
print("Connected...")
while True:
message = client.read();
if len(message) > 0:
header = "Server: "
print(header + message)
def __init__(self):
# init ROS node
rospy.init_node("holo_publisher", anonymous=True)
self.path_sub_ = rospy.Subscriber("path_plan", Path, self.recv_path_)
# streaming nearest n_points in planned path
self.n_points_ = rospy.get_param("n_points", default=100)
self.step_size = rospy.get_param("step_size", default=5)
self.sender_ = TcpClient("192.168.1.176", 12346)
self.q_ = collections.deque(maxlen=3)
self.queue_cond_ = threading.Condition()
def connect_tcp_client(ip, port):
global tcp_conn
global mode
tcp_conn = TcpClient(ip, port)
try:
tcp_conn.run()
if mode == Mode.PI_CONNECTING:
mode = Mode.PI_CONNECTED
while True:
data = tcp_conn.get_json()
if data is None:
break
handle_request(data)
except:
mode = Mode.NONE
mode = Mode.NONE
def __init__(self, mode, parent=None):
QObject.__init__(self, parent)
# ---
self.server = TcpServer(mode, self)
self.server.logMessage.connect(self.emit_log)
self.server.readByteArray.connect(self.on_read_bytearray)
self.pending_data = []
self.stored_data = []
# ---
self.client = TcpClient(self)
self.client.logMessage.connect(self.emit_log)
self.client.readByteArray.connect(self.on_read_bytearray_answer)
self.pending_data_answer = []
self.stored_data_answer = []
# ---
self.iserv = u'КАУС->ПРМК'
self.iserv_2 = u'ПРМК->ПРДК'
self.oserv = u'ПРМК->КАУС'
self.oserv_2 = u'ПРДК->ПРМК'
self.iclient = u'ПРДК->ПРМК'
self.oclient = u'ПРМК->ПРДК'
# ---
self.dest_sign = u'ПРМК->КАУС'
self.dest_sign_2 = u'КАУС->ПРМК'
if mode == ImitParams.transmitter_mode:
self.dest_sign = u'ПРДК->ПРМК'
self.dest_sign_2 = u'ПРМК->ПРДК'
# ---
self.tflag = True if mode == ImitParams.transmitter_mode else False
def do_test(thread_name, host, port, connection_count, loop_per_connection,
file_a, file_b):
send_count = 0
error_count = 0
start_time = time.time()
for k in range(connection_count):
tcp_client = TcpClient(host, port)
for i in range(loop_per_connection):
send_count = send_count + 1
body_data = build_request_data(file_a, file_b)
tcp_client.send(body_data)
status, result_data = tcp_client.recv()
if status != STATUS_OK:
error_count = error_count + 1
print(
f'[{thread_name}] send = {send_count:7}, error = {error_count:7}, result = {result_data}',
end='\r')
tcp_client.close()
elapsed_time = time.time() - start_time
print(
f'[{thread_name}] send = {send_count:7}, error = {error_count:7}, result = {result_data}, elapsed_time = {elapsed_time:.3f} sec'
)
class HoloPublisher(object):
def __init__(self):
# init ROS node
rospy.init_node("holo_publisher", anonymous=True)
self.path_sub_ = rospy.Subscriber("path_plan", Path, self.recv_path_)
# streaming nearest n_points in planned path
self.n_points_ = rospy.get_param("n_points", default=100)
self.step_size = rospy.get_param("step_size", default=5)
self.sender_ = TcpClient("192.168.1.176", 12346)
self.q_ = collections.deque(maxlen=3)
self.queue_cond_ = threading.Condition()
def launch(self):
rate = rospy.Rate(5)
while not rospy.is_shutdown():
self.queue_cond_.acquire()
if len(self.q_) == 0:
self.queue_cond_.wait()
self.sender_.send( self.q_.popleft() )
self.queue_cond_.release()
print("Send Path plan")
rate.sleep()
def recv_path_(self, data):
path = data.poses[::self.step_size]
n_points = min(self.n_points_, len(path))
path_bin = b""
for pose_stamped in path[:n_points]:
position = pose_stamped.pose.position
x_bin = struct.pack(">f", position.y)
y_bin = struct.pack(">f", -1.5)
z_bin = struct.pack(">f", -position.x)
path_bin += b"".join([x_bin, y_bin, z_bin])
packsize_bin = struct.pack(">I", len(path_bin))
pack_bin = packsize_bin + path_bin
self.queue_cond_.acquire()
self.q_.append(pack_bin)
self.queue_cond_.notify()
self.queue_cond_.release()
def __init__(self, config):
self.__otgwClient = TcpClient(config['otgw']['host'], int(config['otgw']['port']))
self.__otgw = OTGW()
self.__otgw_worker_thread = None
self.__config = config
self.__oled = OledController(config['oled']['host'])
class OTGWBridge:
def __init__(self, config):
self.__otgwClient = TcpClient(config['otgw']['host'], int(config['otgw']['port']))
self.__otgw = OTGW()
self.__otgw_worker_thread = None
self.__config = config
self.__oled = OledController(config['oled']['host'])
def run(self):
if self.__otgw_worker_thread:
raise RuntimeError("Already running")
self.__otgw_worker_thread = Thread(target=self.__otgw_worker)
self.__otgw_worker_thread.start()
self.__oled.start()
self.__run_mqtt()
def __run_mqtt(self):
def on_mqtt_connect(client, userdata, flags, rc):
# Subscribe to all topics in our namespace when we're connected. Send out
# a message telling we're online
log.info("Connected with result code " + str(rc))
client.subscribe('{}/#'.format(self.__config['mqtt']['set_topic_namespace']))
client.publish(
topic=self.__config['mqtt']['value_topic_namespace'],
payload="online",
qos=self.__config['mqtt']['qos'],
retain=self.__config['mqtt']['retain'])
mqttc = mqtt.Client("otgw", clean_session=False)
if self.__config['mqtt']['username']:
mqttc.username_pw_set(self.__config["mqtt"]["username"], self.__config["mqtt"]["password"])
mqttc.connect(self.__config["mqtt"]["host"], self.__config["mqtt"]["port"])
def on_disconnect(client, userdata, rc):
if rc != 0:
log.warning("Unexpected MQTT disconnection. Will auto-reconnect")
mqttc.on_connect = on_mqtt_connect
mqttc.on_message = self.__on_mqtt_message
mqttc.on_disconnect = on_disconnect
mqttc.will_set(
topic=self.__config['mqtt']['value_topic_namespace'],
payload="offline",
qos=self.__config['mqtt']['qos'],
retain=True)
self.__mqttc = mqttc
mqttc.loop_forever()
def __on_mqtt_message(self, client, userdata, msg):
log.debug("Received message on topic {} with payload {}".format(msg.topic, str(msg.payload)))
command_generators = {
"set/otgw/room_setpoint/temporary": lambda _: "TT={:.2f}".format(float(_)),
"set/otgw/room_setpoint/constant": lambda _: "TC={:.2f}".format(float(_)),
"set/otgw/control_setpoint/temperature": lambda _: "CS={:.2f}".format(float(_)),
"set/otgw/outside_temperature": lambda _: "OT={:.2f}".format(float(_)),
"set/otgw/hot_water/enable": lambda _: "HW={}".format('1' if _ in self.__true_values else '0'),
"set/otgw/hot_water/temperature": lambda _: "SW={:.2f}".format(float(_)),
"set/otgw/central_heating/enable": lambda _: "CH={}".format('1' if _ in self.__true_values else '0'),
}
# Find the correct command generator from the dict above
command_generator = command_generators.get(msg.topic)
if command_generator:
# Get the command and send it to the OTGW
command = command_generator(msg.payload)
self.__otgw.send_command(command)
__true_values = ('True', 'true', '1', 'y', 'yes')
__lastThermostatValues = {}
def __thermostat_first(self, msg):
if msg.msg in ["dhw_setpoint", "control_setpoint"]:
if msg.msg in self.__lastThermostatValues and msg.thermostatSrc.value != self.__lastThermostatValues[msg.msg]:
if msg.msg == "dhw_setpoint":
command = "SW=0"
if msg.msg == "control_setpoint":
command = "CS=0"
self.__otgw.send_command(command)
self.__lastThermostatValues[msg.msg] = msg.thermostatSrc.value
def __on_otgw_message(self, message):
if config["otgw"]["thermostatFirst"]:
self.__thermostat_first(message)
# if message.msg:
# print(message)
for msg in self.__otgw_translate_message(message):
log.debug("Sending message to topic {} value {}".format(msg[1], msg[2]))
self.__mqttc.publish(
topic=msg[1],
payload=msg[2],
qos=config['mqtt']['qos'],
retain=config['mqtt']['retain'])
def __otgw_translate_message(self, message):
def extractBit(value, number):
rev = value[::-1]
try:
return str(rev[number] == "1")
except:
return str(False)
if message.msg and message.boilerSrc and message.thermostatSrc:
msg = message.msg
topic = "{}/{}".format(self.__config['mqtt']['value_topic_namespace'], msg)
value = message.boilerSrc.value
if msg == "status":
#value.
return iter([
(msg, "{}/fault".format(topic), extractBit(value, 0)),
(msg, "{}/ch_active".format(topic), extractBit(value, 1)),
(msg, "{}/dhw_active".format(topic), extractBit(value, 2)),
(msg, "{}/flame".format(topic), extractBit(value, 3)),
])
else:
return iter([(msg, topic, value)])
else:
return iter([])
def __otgw_worker(self):
self._worker_running = True
line_regex = re.compile(r'^.*[\r\n]+')
buffer = ""
while self._worker_running:
buffer += self.__otgwClient.read()
# Find all the lines in the read data
while True:
m = line_regex.match(buffer)
if not m:
break
line = m.group().rstrip('\r\n')
# log.info("Line: {}".format(line))
operation = self.__otgw.processLine(line)
if operation:
try:
if isinstance(operation, OTGW.Message):
self.__on_otgw_message(message=operation)
self.__oled.on_otgw_message(msg=operation)
elif isinstance(operation, OTGW.Command):
if not operation.processed:
log.info("Sending command: '{}'".format(operation.command))
self.__otgwClient.write(operation.command)
operation.sent = time.time()
else:
log.info("Processed command: {}".format(operation))
except Exception as e:
log.warning(str(e))
# Strip the consumed line from the buffer
buffer = buffer[m.end():]
self._worker_thread = None
import argparse
import torch
import config
from tcp_client import TcpClient
from mcts import MCTS
from model import vamperouge_net
VERSION = "mordred"
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("ip_addr")
parser.add_argument("port")
args = parser.parse_args()
# create IA
print(f"loading vamperouge_{VERSION}")
model = vamperouge_net(config)
model.load_checkpoint("models", f"{VERSION}.pth.tar")
mcts = MCTS(model, config)
# bind IA to client
client = TcpClient(mcts)
# start client
client.connect(args.ip_addr, int(args.port))
client.start()
class Proto(QObject):
__pyqtSignals__ = ('logMessage(QString, int)',
'ready()',
'readyAnswer()')
def __init__(self, mode, parent=None):
QObject.__init__(self, parent)
# ---
self.server = TcpServer(mode, self)
self.server.logMessage.connect(self.emit_log)
self.server.readByteArray.connect(self.on_read_bytearray)
self.pending_data = []
self.stored_data = []
# ---
self.client = TcpClient(self)
self.client.logMessage.connect(self.emit_log)
self.client.readByteArray.connect(self.on_read_bytearray_answer)
self.pending_data_answer = []
self.stored_data_answer = []
# ---
self.iserv = u'КАУС->ПРМК'
self.iserv_2 = u'ПРМК->ПРДК'
self.oserv = u'ПРМК->КАУС'
self.oserv_2 = u'ПРДК->ПРМК'
self.iclient = u'ПРДК->ПРМК'
self.oclient = u'ПРМК->ПРДК'
# ---
self.dest_sign = u'ПРМК->КАУС'
self.dest_sign_2 = u'КАУС->ПРМК'
if mode == ImitParams.transmitter_mode:
self.dest_sign = u'ПРДК->ПРМК'
self.dest_sign_2 = u'ПРМК->ПРДК'
# ---
self.tflag = True if mode == ImitParams.transmitter_mode else False
def pop_pending_data(self):
try:
pdata = self.pending_data.pop(0)
return (pdata[0], pdata[1])
except IndexError:
return None
def pop_pending_data_prdk(self):
try:
pdata = self.pending_data_answer.pop(0)
return (pdata[0], pdata[1])
except IndexError:
return None
def send_data(self, cmd, params):
array, dgram = self.create_datagram(DATAGRAM_ANS4AUS, DEFAULT_SESSION_ID, [cmd] + params)
dest = self.oserv
if self.tflag:
dest = self.oserv_2
self.emit_log(u'%s: %s' % (dest, dgram), 110)
self.server.write(array)
def send_data_prdk(self, cmd, params):
array, dgram = self.create_datagram(DATAGRAM_ANS4AUS, DEFAULT_SESSION_ID, [cmd] + params)
self.emit_log(u'ПРМК->ПРДК: %s' % dgram, 110)
self.client.write(array)
def create_datagram(self, dtype, dservice, info):
array = QByteArray()
array.append(chr(dtype))
info_size = len(info)
hi = (info_size >> 8) & 0xFF
lo = info_size & 0xFF
array.append(chr(hi))
array.append(chr(lo))
array.append(chr(dservice))
for byte in info:
array.append(chr(byte))
array.insert(0, chr(MARKER))
dgram = bytearray2str(array)
return (array, dgram)
def on_read_bytearray_answer(self, bytearray):
# --- переводим QByteArray в список int
ilist = bytearray2intlist(bytearray)
# --- выполняем последовательный поиск МАРКЕРОВ
from_index = 0
marker_index = -1
while True:
try:
marker_index = ilist.index(MARKER, from_index)
# --- если маркер не первый - видимо это завершение предыдущей КДГ
# --- или просто мусор (зависит от сохраненных ранее байтов)
if from_index == 0 and marker_index > 0:
if len(self.stored_data_answer) != 0: # имеются сохраненные данные => это завершение начатого пакета
self.stored_data_answer += ilist[0:marker_index] # прицепляем "хвост"
info = self.check_datagram_validation(self.stored_data_answer, False)
if info and len(info) > 0:
self.pending_data_answer.append((info[0], info[1:]))
self.emit(SIGNAL('readyAnswer()'))
self.stored_data_answer = []
from_index = marker_index + 1
continue
# ---
if len(self.stored_data_answer) != 0:
self.emit_log(u'Данные отброшены (%d) - %s'
% (len(self.stored_data_answer), intlist2str(self.stored_data_answer)))
self.stored_data_answer = []
# ---
kdg = self.check_datagram_validation(ilist[marker_index:], False)
# ---
if not kdg:
from_index = marker_index + 1
continue
# ---
if len(kdg) == 0:
self.stored_data_answer = ilist[marker_index:]
from_index = marker_index + 1
continue
# ---
cmd = kdg[INFO_OFFSET]
params = kdg[INFO_OFFSET + 1:]
pdata = (cmd, params)
self.pending_data_answer.append(pdata)
self.emit(SIGNAL('readyAnswer()'))
from_index = marker_index + len(kdg)
except ValueError:
if marker_index < 0:
if len(self.stored_data_answer) > 0:
self.stored_data_answer += ilist
break
# --- обрабатываем данные от сервера
def on_read_bytearray(self, bytearray):
# --- переводим QByteArray в список int
ilist = bytearray2intlist(bytearray)
# --- выполняем последовательный поиск МАРКЕРОВ
from_index = 0
marker_index = -1
while True:
try:
marker_index = ilist.index(MARKER, from_index)
# --- если маркер не первый - видимо это завершение предыдущей КДГ
# --- или просто мусор (зависит от сохраненных ранее байтов)
if from_index == 0 and marker_index > 0:
if len(self.stored_data) != 0: # имеются сохраненные данные => это завершение начатого пакета
self.stored_data += ilist[0:marker_index] # прицепляем "хвост"
info = self.check_datagram_validation(self.stored_data)
if info and len(info) > 0:
self.pending_data.append((info[0], info[1:]))
self.emit(SIGNAL('ready()'))
self.stored_data = []
from_index = marker_index + 1
continue
# ---
if len(self.stored_data) != 0:
self.emit_log(u'Данные отброшены (%d) - %s'
% (len(self.stored_data), intlist2str(self.stored_data)))
self.stored_data = []
# ---
kdg = self.check_datagram_validation(ilist[marker_index:])
# ---
if not kdg:
from_index = marker_index + 1
continue
# ---
if len(kdg) == 0:
self.stored_data = ilist[marker_index:]
from_index = marker_index + 1
continue
# ---
self.pending_data.append((kdg[INFO_OFFSET], kdg[INFO_OFFSET + 1:]))
self.emit(SIGNAL('ready()'))
from_index = marker_index + len(kdg)
except ValueError:
if marker_index < 0:
if len(self.stored_data) > 0:
self.stored_data += ilist
break
def check_datagram_validation(self, pack, Server_Flag=True):
"""
Проверяем датаграмму на валидность
pack = [int, int, ...]
"""
dgram = intlist2str(pack)
if len(pack) < HEADER_SIZE:
self.emit_log(u'Недостаточная длина пакета - %s' % dgram, 130)
return None
du = self.get_info_length(pack)
df = len(pack) - HEADER_SIZE
if du > df:
return [] # незавершенная кодограмма
dest = self.iserv
if self.tflag:
dest = self.iserv_2
elif not Server_Flag:
dest = self.iclient
self.emit_log(u'%s: %s' % (dest, dgram), 110)
dtype = self.get_datagram_type(pack)
dservice = self.get_service_type(pack)
if dtype == DATAGRAM_NULL:
return None
elif dtype == DATAGRAM_REG4KRS:
if Server_Flag:
self.emit_log(u'Получен пакет регистрации, тип сеанса - %d' % dservice)
array, dgram = self.create_datagram(DATAGRAM_SOK4AUS, DEFAULT_SESSION_ID, [])
dest = self.iserv
if self.tflag:
dest = self.iserv_2
self.emit_log(u'%s: %s' % (dest, dgram), 110)
self.server.write(array)
self.emit_log(u'Сеанс управления открыт, тип сеанса - %d' % DEFAULT_SESSION_ID)
else:
self.emit_log(u'Ошибка регистрации - неожиданное сообщение', 130)
return None
elif dtype == DATAGRAM_CMD4KRS:
kdg = pack[:INFO_OFFSET + du]
return kdg
elif dtype == DATAGRAM_ANS4AUS:
if len(pack) <= HEADER_SIZE:
return None
kdg = pack[:INFO_OFFSET + du]
return kdg
else:
#self.emit_log(u'Ошибка типа входящего сообщения (%d)' % dtype)
return None
def get_datagram_type(self, pack):
return pack[TYPE_OFFSET]
def get_service_type(self, pack):
return pack[SERVICE_OFFSET]
def get_info_length(self, pack):
hi = pack[HIGH_LENGTH_OFFSET] << 8
low = pack[LOW_LENGTH_OFFSET]
length = hi | low
return length
def emit_log(self, text, type_=10):
self.emit(SIGNAL('logMessage(QString, int)'), text, type_)
