class WsServer(threading.Thread):
def __init__(self, conf=None, controller=None):
threading.Thread.__init__(self)
if conf == None:
self.conf = dict(port=4000, host='127.0.0.1')
else:
self.conf = conf
self.server = WebsocketServer(self.conf["port"],
host=self.conf["host"])
self.server.set_fn_new_client(self.new_client)
self.server.set_fn_client_left(self.client_left)
self.server.set_fn_message_received(self.message_received)
def run(self):
self.server.run_forever()
def new_client(client, server):
logging.info("WS: New Client")
self.server.send_message_to_all("Hey all, a new client has joined us")
def client_left(client, server):
logging.info("WS: Client left")
self.server.send_message_to_all("Hey all, a new client has joined us")
def message_received(client, server, message):
logging.info("WS: New message " + message)
def disconnect(self):
logging.info("WS:Disconnect")
self.server.shutdown()
class WebsocketThread(Thread):
def __init__(self):
Thread.__init__(self)
self.websocket = Websocket_Server()
self.server = WebsocketServer(config.WS_PORT,loglevel=logging.INFO)
self.client = self.websocket.new_client
self.server.set_fn_new_client(self.client)
self.server.set_fn_message_received(self.websocket.message_received)
def run(self):
self.server.run_forever()
def shutdown(self):
self.server.shutdown()
class WSServer:
def __init__(self):
self.serv = None
self.clients = []
self.previous_messages = []
def start(self):
port = 4001
self.serv = WebsocketServer(port, "0.0.0.0")
self.serv.set_fn_new_client(self.client_joined)
self.serv.set_fn_message_received(self.message_received)
self.serv.set_fn_client_left(self.client_left)
thr = Thread(target=lambda: self.serv.run_forever())
thr.start()
def stop(self):
self.serv.shutdown()
self.serv.server_close()
def send_ws_message(self, msg):
if not self.is_enabled():
return
self.previous_messages.append(msg)
self.serv.send_message_to_all(msg)
def is_enabled(self):
return self.serv is not None
def client_joined(self, client, server):
if len(self.previous_messages) == 0:
return
server.send_message(client, "payload-start")
for prev_msg in self.previous_messages:
server.send_message(client, prev_msg)
server.send_message(client, "payload-end")
def message_received(self, client, server):
pass
def client_left(self, client, server):
pass
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
return ch
def startServer(server):
print("HMI signals listener was started\r")
server.set_fn_new_client(new_client)
server.set_fn_client_left(client_left)
server.set_fn_message_received(message_received)
server.run_forever()
def keyBoardEvent():
global server
char = ' '
while char != 'q':
char = getch()
server = WebsocketServer(8081)
serverThread = Thread(target=startServer, args=(server, ))
keyBoardThread = Thread(target=keyBoardEvent)
keyBoardThread.start()
serverThread.start()
keyBoardThread.join()
print("Closing server...")
server.shutdown()
server.server_close()
class WBmessenger(object):
def __init__(self, viewerparent):
self.parent = viewerparent
self.ProcessMessage = self.parent.ProcessMessage
self.websockport = self.parent.websockport
self.sleeptime = self.parent.sleeptime
self.mprint = self.parent.mprint
self.parent.lastviewmtrx
self.browserisopen = False
self.msgqueue = []
self.msgdelim = ":\n"
self.ishandling = False
self.websockclient = None
self.isterminating = False
self.was_disconnected = None
self.mywebsock = None
self.websockeventloop = None
def Sleep(self, t):
time.sleep(t)
def OnWebsocketClientMessage(self, client, server, message):
self.ProcessMessage(message)
def StartWebsocket(self):
self.server = WebsocketServer(self.websockport, host='127.0.0.1')
if not self.server:
raise Sorry("Could not connect to web browser")
self.server.set_fn_new_client(self.OnConnectWebsocketClient)
self.server.set_fn_client_left(self.OnDisconnectWebsocketClient)
self.server.set_fn_message_received(self.OnWebsocketClientMessage)
self.wst = threading.Thread(target=self.server.run_forever)
self.wst.daemon = True
self.wst.start()
self.msgqueuethrd = threading.Thread(target=self.WebBrowserMsgQueue)
self.msgqueuethrd.daemon = True
self.msgqueuethrd.start()
def StopWebsocket(self):
try:
if self.websockclient: # might not have been created if program is closed before a data set is shown
self.websockclient['handler'].send_text(u"", opcode=0x8)
except Exception as e:
self.mprint(to_str(e) + "\n" + traceback.format_exc(limit=10),
verbose=0)
self.mprint("Shutting down Websocket listening thread", verbose=1)
self.server.shutdown()
self.parent.javascriptcleaned = True
self.msgqueuethrd.join()
self.mprint("Shutting down WebsocketServer", verbose=1)
self.wst.join()
self.isterminating = True
def AddToBrowserMsgQueue(self, msgtype, msg=""):
self.msgqueue.append((msgtype, msg))
def WebBrowserMsgQueue(self):
try:
while True:
nwait = 0.0
time.sleep(self.sleeptime)
if self.parent.javascriptcleaned:
self.mprint("Shutting down WebBrowser message queue",
verbose=1)
return
if len(self.msgqueue):
pendingmessagetype, pendingmessage = self.msgqueue[0]
gotsent = self.send_msg_to_browser(pendingmessagetype,
pendingmessage)
while not self.browserisopen: #self.websockclient:
time.sleep(self.sleeptime)
nwait += self.sleeptime
if nwait > self.parent.handshakewait or self.parent.javascriptcleaned or not self.viewerparams.scene_id is not None:
return
if gotsent:
self.msgqueue.remove(self.msgqueue[0])
#if self.was_disconnected:
# nwait2 = 0.0
# while nwait2 < self.parent.handshakewait:
# nwait2 += self.sleeptime
# self.ReloadNGL()
# if the html content is huge the browser will be unresponsive until it has finished
# reading the html content. This may crash this thread. So try restarting this thread until
# browser is ready
except Exception as e:
self.mprint( str(e) + ", Restarting WebBrowserMsgQueue\n" \
+ traceback.format_exc(limit=10), verbose=2)
self.websockclient = None
self.WebBrowserMsgQueue()
def OnConnectWebsocketClient(self, client, server):
self.websockclient = client
self.mprint("Browser connected:" + str(self.websockclient), verbose=1)
if self.was_disconnected:
self.was_disconnected = False
if self.parent.lastviewmtrx and self.parent.viewerparams.scene_id is not None:
self.parent.set_volatile_params()
self.mprint("Reorienting client after refresh:" +
str(self.websockclient),
verbose=2)
self.AddToBrowserMsgQueue("ReOrient", self.parent.lastviewmtrx)
else:
self.parent.SetAutoView()
def OnDisconnectWebsocketClient(self, client, server):
self.mprint("Browser disconnected:" + str(client), verbose=1)
self.was_disconnected = True
def send_msg_to_browser(self, msgtype, msg=""):
message = u"" + msgtype + self.msgdelim + str(msg)
if self.websockclient:
nwait = 0.0
while not ("Ready" in self.parent.lastmsg or "tooltip_id" in self.parent.lastmsg \
or "CurrentViewOrientation" in self.parent.lastmsg or "AutoViewSet" in self.parent.lastmsg \
or "ReOrient" in self.parent.lastmsg or "JavaScriptCleanUp" in self.parent.lastmsg or self.websockclient is None):
time.sleep(self.sleeptime)
nwait += self.sleeptime
if nwait > 2.0 and self.browserisopen:
self.mprint("ERROR: No handshake from browser!", verbose=0)
self.mprint("failed sending " + msgtype, verbose=1)
self.mprint("Reopening webpage again", verbose=0)
break
if self.browserisopen and self.websockclient is not None:
try:
self.server.send_message(self.websockclient, message)
return True
except Exception as e:
self.mprint(str(e) + "\n" + traceback.format_exc(limit=10),
verbose=1)
self.websockclient = None
return False
else:
return self.parent.OpenBrowser()
class FakeGWServer(threading.Thread):
"""
Creates an FakeGWServer.
Arguments:
port -- port of WebSocket connection.
config -- configuration of behaviour, could contain following parameters:
'autoRegister' -- automatic registration accepting
'autoResponse' -- automatic response for requests
'autoData' -- automatic data messages processing
'prePaired' -- list of paired devices for device_list requests.
'autoNewDevice' -- automatic new device reqests handling
"""
def __init__(self, port, config, host='127.0.0.1'):
self.autoRegister = config['autoRegister']
self.autoResponse = config['autoResponse']
self.autoData = config['autoData']
self.prePaired = config['prePaired']
self.autoNewDevice = config['autoNewDevice']
self.WSServer = WebsocketServer(port, host, loglevel=logging.DEBUG)
self.WSServer.set_fn_message_received(self.message_received)
self.WSServer.set_fn_client_left(self.client_left)
self.WSServer.set_fn_new_client(self.new_client)
self.messages = []
self.dataMessages = []
self.responseMessages = []
self.newDevices = []
self.client = None
self.clientLock = threading.Lock()
self.newConnection = threading.Condition(self.clientLock)
self.disconnect = threading.Condition(self.clientLock)
self.newMessage = threading.Condition(self.clientLock)
self.newDataMessage = threading.Condition(self.clientLock)
self.newResponseMessage = threading.Condition(self.clientLock)
self.newDeviceMessage = threading.Condition(self.clientLock)
self.connected = False
self.resetEnable = False
super().__init__()
def run(self):
self.WSServer.run_forever()
self.WSServer.socket.close()
def stop(self):
self.WSServer.shutdown()
def enableReset(self):
with self.clientLock:
self.resetEnable = True
def new_client(self, client, server):
with self.clientLock:
if self.connected:
logging.debug("Unknown client refused")
client['handler'].send_text("",
websocket_server.OPCODE_CLOSE_CONN)
elif self.client is None or self.resetEnable:
self.client = client
if not self.autoRegister:
self.connected = True
self.newConnection.notify_all()
self.resetEnable = False
logging.debug("connection established")
else:
logging.debug("dropping completely")
client['handler'].send_text("",
websocket_server.OPCODE_CLOSE_CONN)
def client_left(self, client, server):
with self.clientLock:
if client == self.client:
logging.debug("client disconnected")
self.connected = False
self.messages = []
self.disconnect.notify_all()
else:
logging.debug("unknown client disconnected")
def message_received(self, client, server, message):
logging.debug("received: " + message)
with self.clientLock:
if self.client != client:
logging.debug("dropping message from unknown client")
return
try:
jsonMessage = json.loads(message)
if self.autoRegister:
if jsonMessage['message_type'] == "gateway_register":
self.WSServer.send_message(
self.client,
'{ "message_type" : "gateway_accepted" }')
self.connected = True
self.newConnection.notify_all()
return
if self.autoData:
if jsonMessage['message_type'] == "sensor_data_export":
sensorData = SensorData.SensorData()
sensorData.fromJSON(jsonMessage['data'][0])
self.dataMessages.append(sensorData)
self.newDataMessage.notify_all()
response = {}
response['message_type'] = "sensor_data_confirm"
response['id'] = jsonMessage['id']
logging.debug("sending message: " +
json.dumps(response))
self.WSServer.send_message(self.client,
json.dumps(response))
return
if self.prePaired is not None:
if jsonMessage['message_type'] == "device_list_request":
if jsonMessage['device_prefix'] in self.prePaired:
response = {}
response['message_type'] = "device_list_response"
response['id'] = jsonMessage['id']
response['status'] = 1
response['devices'] = self.prePaired[
jsonMessage['device_prefix']]
logging.debug("sending message: " +
json.dumps(response))
self.WSServer.send_message(self.client,
json.dumps(response))
return
if self.autoResponse:
if jsonMessage['message_type'] == "response_with_ack":
self.responseMessages.append(jsonMessage)
self.newResponseMessage.notify_all()
response = {}
response['message_type'] = "generic_ack"
response['id'] = jsonMessage['id']
response['status'] = jsonMessage['status']
logging.debug("sending message: " +
json.dumps(response))
self.WSServer.send_message(self.client,
json.dumps(response))
return
if jsonMessage['message_type'] == "generic_response":
self.responseMessages.append(jsonMessage)
self.newResponseMessage.notify_all()
return
if self.autoNewDevice:
if jsonMessage['message_type'] == "new_device_request":
device = Device.Device(jsonMessage)
self.newDevices.append(device)
self.newDeviceMessage.notify_all()
response = {}
response['message_type'] = "generic_ack"
response['id'] = jsonMessage['id']
response['status'] = 1
logging.debug("sending message: " +
json.dumps(response))
self.WSServer.send_message(self.client,
json.dumps(response))
return
except Exception as ex:
logging.error("Exception: " + str(ex))
pass
self.messages.append(message)
self.newMessage.notify_all()
def nextNewDevice(self, timeout=10):
with self.clientLock:
if not self.newDevices:
self.newDeviceMessage.wait(timeout)
if self.newDevices:
return self.newDevices.pop(0)
def nextResponse(self, timeout=10):
with self.clientLock:
if not self.responseMessages:
self.newResponseMessage.wait(timeout)
if self.responseMessages:
return self.responseMessages.pop(0)
def nextData(self, timeout=10):
with self.clientLock:
if not self.dataMessages:
self.newDataMessage.wait(timeout)
if self.dataMessages:
return self.dataMessages.pop(0)
def clearData(self):
with self.clientLock:
self.dataMessages = []
def nextMessage(self, timeout=10):
with self.clientLock:
if not self.messages:
self.newMessage.wait(timeout)
if self.messages:
return self.messages.pop(0)
def waitForDisconnect(self, timeout=10):
with self.clientLock:
if not self.connected:
return True
self.disconnect.wait(timeout)
return not self.connected
def waitForConnection(self, timeout=10):
with self.clientLock:
if self.connected:
return True
self.newConnection.wait(timeout)
return self.connected
def sendMessage(self, message):
with self.clientLock:
if self.connected:
logging.debug("sending message: " + message)
self.WSServer.send_message(self.client, message)
else:
raise Exception("client is not connected")
class WebsocketServerModule(ModuleProcess):
def __init__(self, baseConfig, pInBoundQueue, pOutBoundQueue,
loggingQueue):
# super(WebsocketServerModule, self).__init__()
ModuleProcess.__init__(self, baseConfig, pInBoundQueue, pOutBoundQueue,
loggingQueue)
self.alive = False
self.config = baseConfig
self.inQueue = pInBoundQueue # inQueue are messages from the main process to websocket clients
self.outQueue = pOutBoundQueue # outQueue are messages from clients to main process
self.websocketServer = None
self.loggingQueue = loggingQueue
self.threadProcessQueue = None
# Configs
self.moduleConfig = configLoader.load(self.loggingQueue, __name__)
# Constants
self._port = self.moduleConfig['WebsocketPort']
self._host = self.moduleConfig['WebsocketHost']
# logging setup
self.logger = ThreadsafeLogger(loggingQueue, __name__)
def run(self):
if not self.check_ss_version():
#cant run with wrong version so we return early
return False
""" Main thread entry point.
Sets up websocket server and event callbacks.
Starts thread to monitor inbound message queue.
"""
self.logger.info("Starting websocket server")
self.alive = True
self.listen()
self.websocketServer = WebsocketServer(self._port, host=self._host)
self.websocketServer.set_fn_new_client(self.new_websocket_client)
self.websocketServer.set_fn_message_received(
self.websocket_message_received)
self.websocketServer.run_forever()
def check_ss_version(self):
#check for min version met
self.logger.info('Module version %s' % (__version__))
if LooseVersion(self.config['ss_version']) < LooseVersion(
self.moduleConfig['MinSimpleSensorVersion']):
self.logger.error(
'This module requires a min SimpleSensor %s version. This instance is running version %s'
% (self.moduleConfig['MinSimpleSensorVersion'],
self.config['ss_version']))
return False
return True
def new_websocket_client(self, client, server):
""" Client joined callback - called whenever a new client joins. """
self.logger.debug("Client joined")
def websocket_message_received(self, client, server, message):
""" Message received callback - called whenever a new message is received. """
self.logger.debug('Message received: %s' % message)
message = json.loads(message)
self.logger.info("message jsond: %s" % message)
_msg = Message(topic=message['topic'], sender_id=message['sender_id'])
if 'sender_type' in message:
_msg.sender_type = message['sender_type']
if 'recipients' in message:
_msg.recipients = message['recipients']
if 'extended_data' in message:
_msg.extended_data = message['extended_data']
self.put_message(_msg)
def listen(self):
self.threadProcessQueue = Thread(target=self.process_queue)
self.threadProcessQueue.setDaemon(True)
self.threadProcessQueue.start()
def shutdown(self):
""" Handle shutdown message.
Close and shutdown websocket server.
Join queue processing thread.
"""
self.logger.info("Shutting down websocket server")
try:
self.logger.info("Closing websocket")
self.websocketServer.server_close()
except Exception as e:
self.logger.error("Websocket close error : %s " % e)
try:
self.logger.info("Shutdown websocket")
self.websocketServer.shutdown()
except Exception as e:
self.logger.error("Websocket shutdown error : %s " % e)
self.alive = False
self.threadProcessQueue.join()
time.sleep(1)
self.exit = True
def handle_message(self, message):
""" Send message to listening clients. """
self.websocketServer.send_message_to_all(json.dumps(message.__dict__))
def process_queue(self):
""" Monitor queue of messages from main process to this thread. """
while self.alive:
if (self.inQueue.empty() == False):
try:
message = self.inQueue.get(block=False, timeout=1)
if message is not None:
if message.topic.upper() == "SHUTDOWN":
self.logger.debug("SHUTDOWN handled")
self.shutdown()
else:
self.handle_message(message)
except Exception as e:
self.logger.error("Websocket unable to read queue : %s " %
e)
else:
time.sleep(.25)
def test_websocket():
"""
Start a ws.Client and check basic functionality.
"""
serverQ = Queue(1)
clientQ = Queue(1)
errorQ = Queue(1)
def receive(client, server, message):
if message == HELLO:
server.send_message(client, YO)
elif message == QUIT:
client["handler"].request.send(CLOSE_HANDSHAKE)
server.shutdown()
serverQ.put(message)
server = WebsocketServer(PORT, HOST)
server.set_fn_message_received(receive)
serverThread = threading.Thread(None, server.run_forever)
serverThread.start()
time.sleep(1)
class test:
was_opened = False
was_closed = False
def on_open(ws):
test.was_opened = True
def on_message(ws, msg):
clientQ.put(msg, timeout=1)
def on_close(ws):
test.was_closed = True
def on_error(ws, error):
errorQ.put(error, timeout=1)
def client(url):
return ws.Client(
url=url,
on_open=on_open,
on_message=on_message,
on_close=on_close,
on_error=on_error,
)
try:
# Send a hello and make sure it is received.
cl = client(URL)
assert test.was_opened
cl.send(HELLO)
msg = serverQ.get(timeout=1)
assert msg == HELLO
# Make sure we got the response.
msg = clientQ.get(timeout=1)
assert msg == YO
# Trigger a server shutdown.
cl.send(QUIT)
serverQ.get(timeout=1)
# Make sure the close callback was called.
assert test.was_closed
# Make sure a send fails.
with pytest.raises(websocket.WebSocketConnectionClosedException):
cl.send("should be closed")
# Force a client error and ensure it comes through the callback.
assert errorQ.empty()
cl = client("notaurl")
errorQ.get(timeout=1)
except Exception as e:
server.shutdown()
raise e
finally:
cl.close()
serverThread.join()
time.sleep(1.0)
print("Data post thread stopped")
is_active = True
mainView = UIMainView() if no_lcd is False else UIConsoleView()
dataThread = threading.Thread(target=dataThreadFunc)
dataThread.start()
websocket = WebsocketServer(PORT_NUMBER_WS, host="0.0.0.0")
weThread = threading.Thread(target=websocketThreadFunc)
weThread.start()
httpd = HTTPServer(('', PORT_NUMBER), HTTPHandler)
serverThread = threading.Thread(target=httpServerFunc)
serverThread.start()
postThread = threading.Thread(target=postThreadFunc)
postThread.start()
# Run UI
mainView.mainloop()
is_active = False
httpd.shutdown()
websocket.shutdown()
print("App done")
class WebTunnel(threading.Thread):
def __init__(self, controller, port):
threading.Thread.__init__(self)
self.port = port
self.outboundPackets = {
"SET_VALUE_BOUNDS": "svb",
"FEEDBACK_VALUE": "fv" #aka true value
}
self.inboundPackets = {"USER_VALUE": "uv"}
self.positionSuffix = "0"
self.maxVelocitySuffix = "1"
self.stiffnessSuffix = "2"
self.jointNoToWidgetPrefix = ["joint0_", "joint1_"]
self.controller = controller
self.queue = []
self.server = WebsocketServer(self.port)
self.server.set_fn_new_client(self.new_client)
self.server.set_fn_client_left(self.client_left)
self.server.set_fn_message_received(self.message_received)
self.positionFeedback = {}
self.stiffnessFeedback = {}
self.positionBounds = {}
self.stiffnessBounds = {}
self.maxVelocityBounds = {}
def run(self):
print("Tunnel now running!")
self.server.run_forever()
def shutdown(self):
print("Shutting down")
try:
self.server.shutdown()
except:
exit(1)
# Called for every client connecting (after handshake)
def new_client(self, client, server):
print("New client connected and was given id %d" % client['id'])
print("Filling in client with latest data")
#send latest position bounds
for k in self.positionBounds:
self.server.send_message(
client,
self.getPositionBoundsPacket(k, str(self.positionBounds[k][0]),
self.positionBounds[k][1]))
#send latest position feedbacks
for k in self.positionFeedback:
self.server.send_message(
client,
self.getPositionFeedbackPacket(k, self.positionFeedback[k]))
#send latest max velocity bounds
for k in self.maxVelocityBounds:
self.server.send_message(
client,
self.getMaxVelocityBoundsPacket(
k, str(self.maxVelocityBounds[k][0]),
self.maxVelocityBounds[k][1]))
#send latest stiffness bounds
for k in self.stiffnessBounds:
self.server.send_message(
client,
self.getStiffnessBoundsPacket(k,
str(self.stiffnessBounds[k][0]),
self.stiffnessBounds[k][1]))
#send latest applied force feedbacks
#for k in self.stiffnessFeedback:
# self.server.send_message(client, self.getStiffnessFeedbackPacket(k,
# self.stiffnessFeedback[k]))
def client_left(self, client, server):
print("Client(%d) disconnected" % client['id'])
def message_received(self, client, server, message):
print("Client %d said: %s", client['id'], message)
if '%' in message:
tokens = message.split('%')
if tokens[0] == self.inboundPackets['USER_VALUE']:
if tokens[1] and tokens[2]:
jointType = tokens[1].split("_")
jointType = jointType[len(jointType) - 1]
jointPrefix = tokens[1].split("_")[0] + "_"
#jointNo = jointPrefix.split("joint")
#jointNo = jointNo[len(jointNo) - 1]
print "jointPrefix: " + jointPrefix + "\n"
print "joint type: " + jointType + "\n"
if jointPrefix in self.jointNoToWidgetPrefix:
index = self.jointNoToWidgetPrefix.index(jointPrefix)
if self.controller:
if jointType == self.positionSuffix:
self.controller.setPosition(index, tokens[2])
elif jointType == self.maxVelocitySuffix:
self.controller.setMaxVelocity(
index, tokens[2])
elif jointType == self.stiffnessSuffix:
self.controller.setStiffness(index, tokens[2])
else:
print(
'Undefined controller -- tried calling setPosition('
+ str(index) + ', ' + str(tokens[2]) + ')')
else:
print('Could not find target with prefix: ' +
jointPrefix)
else:
print('Corrupt USER_VALUE packet')
else:
print('Unhandled packet: ' + tokens[0])
elif message == 'PING':
self.server.send_message_to_all('PONG')
else:
print('Invalid packet')
def sendPositionBoundsPacket(self, no, lower, upper):
self.server.send_message_to_all(
self.getPositionBoundsPacket(no, lower, upper))
self.positionBounds[no] = [lower, upper]
def sendPositionFeedbackPacket(self, no, val):
self.server.send_message_to_all(self.getPositionFeedbackPacket(
no, val))
self.positionFeedback[no] = val
def sendStiffnessBoundsPacket(self, no, lower, upper):
print "SENDING STIFFNESS BOUNDS: " + str(no) + " " + str(
lower) + " " + str(upper)
self.server.send_message_to_all(
self.getStiffnessBoundsPacket(no, lower, upper))
self.stiffnessBounds[no] = [lower, upper]
def sendStiffnessFeedbackPacket(self, no, val):
self.server.send_message_to_all(
self.getStiffnessFeedbackPacket(no, val))
self.stiffnessFeedback[no] = val
def sendMaxVelocityBoundsPacket(self, no, lower, upper):
self.server.send_message_to_all(
self.getMaxVelocityBoundsPacket(no, lower, upper))
self.maxVelocityBounds[no] = [lower, upper]
def getPositionBoundsPacket(self, no, lower, upper):
if self.jointNoToWidgetPrefix[no]:
return (self.outboundPackets["SET_VALUE_BOUNDS"] + "%" +
self.jointNoToWidgetPrefix[no] + self.positionSuffix +
"%" + str(lower) + "%" + str(upper))
else:
print("Could not find widget prefix for no = " + no)
return None
def getStiffnessBoundsPacket(self, no, lower, upper):
if self.jointNoToWidgetPrefix[no]:
return (self.outboundPackets["SET_VALUE_BOUNDS"] + "%" +
self.jointNoToWidgetPrefix[no] + self.stiffnessSuffix +
"%" + str(lower) + "%" + str(upper))
else:
print("Could not find widget prefix for no = " + no)
return None
def getPositionFeedbackPacket(self, no, val):
if self.jointNoToWidgetPrefix[no]:
return (self.outboundPackets["FEEDBACK_VALUE"] + "%" +
self.jointNoToWidgetPrefix[no] + str(self.positionSuffix) +
"%" + str(val))
else:
print("Could not find widget prefix for no = " + no)
return None
def getStiffnessFeedbackPacket(self, no, val):
if self.jointNoToWidgetPrefix[no]:
return (self.outboundPackets["FEEDBACK_VALUE"] + "%" +
self.jointNoToWidgetPrefix[no] +
str(self.stiffnessSuffix) + "%" + str(val))
else:
print("Could not find widget prefix for no = " + no)
return None
def getMaxVelocityBoundsPacket(self, no, lower, upper):
if self.jointNoToWidgetPrefix[no]:
return (self.outboundPackets["SET_VALUE_BOUNDS"] + "%" +
self.jointNoToWidgetPrefix[no] + self.maxVelocitySuffix +
"%" + str(lower) + "%" + str(upper))
else:
print("Could not find widget prefix for no = " + no)
return None
print(config)
print('config updated!')
except Exception as e:
pass
class WSThread(threading.Thread):
def __init__(self, server):
threading.Thread.__init__(self)
self.server = server
def run(self):
self.server.run_forever()
PORT = 12758
cfg_server = WebsocketServer(PORT)
cfg_server.set_fn_message_received(message_received)
t_ws = WSThread(cfg_server)
t_ws.start()
print('SLEEP')
time.sleep(10)
cfg_server.shutdown()
t_ws.join()
print('SHUTDOWN')
tty.setraw(sys.stdin.fileno())
ch = sys.stdin.read(1)
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
return ch
def startServer(server):
print("HMI signals listener was started\r")
server.set_fn_new_client(new_client)
server.set_fn_client_left(client_left)
server.set_fn_message_received(message_received)
server.run_forever()
def keyBoardEvent():
global server
char = ' '
while char != 'q':
char = getch()
server = WebsocketServer(8081)
serverThread = Thread(target = startServer, args = (server, ))
keyBoardThread = Thread(target = keyBoardEvent)
keyBoardThread.start()
serverThread.start()
keyBoardThread.join()
print("Closing server...")
server.shutdown()
server.server_close()
class RTC_Spider2020_Info(OpenRTM_aist.DataFlowComponentBase):
##
# @brief constructor
# @param manager Maneger Object
#
def __init__(self, manager):
OpenRTM_aist.DataFlowComponentBase.__init__(self, manager)
self._d_GCCapacityRatio_In = OpenRTM_aist.instantiateDataType(
RTC.TimedDouble)
"""
ゲームコントローラのバッテリ残容量比
- Unit: [%]
"""
self._GCCapacityRatio_InIn = OpenRTM_aist.InPort(
"GCCapacityRatio_In", self._d_GCCapacityRatio_In)
self._d_MainCapacityRatio_In = OpenRTM_aist.instantiateDataType(
RTC.TimedDouble)
"""
メインバッテリのバッテリ残容量比
- Unit: [%]
"""
self._MainCapacityRatio_InIn = OpenRTM_aist.InPort(
"MainCapacityRatio_In", self._d_MainCapacityRatio_In)
self._d_Mode_In = OpenRTM_aist.instantiateDataType(RTC.TimedLongSeq)
"""
動作指令の状態
- Number: 6
- Semantics: [0]: 操作モード
[1]: 走行操作 速度レベル
[2]: 走行操作 最大速度フラグ
[3]: 走行モード フリッパーの選択状態
[4]: 走行モード フリッパーの動作状態
[5]: 走行モード 台車の位置姿勢のクリア
"""
self._Mode_InIn = OpenRTM_aist.InPort("Mode_In", self._d_Mode_In)
self._d_Pose3D_In = OpenRTM_aist.instantiateDataType(RTC.TimedPose3D)
"""
３次元の位置・姿勢
- Unit: 位置:[m], 姿勢:[rad]
"""
self._Pose3D_InIn = OpenRTM_aist.InPort("Pose3D_In", self._d_Pose3D_In)
self._d_Velocity2D_In = OpenRTM_aist.instantiateDataType(
RTC.TimedVelocity2D)
"""
メインクローラの速度
- Unit: [m/s]
"""
self._Velocity2D_InIn = OpenRTM_aist.InPort("Velocity2D_In",
self._d_Velocity2D_In)
self._d_FlipperAngle_In = OpenRTM_aist.instantiateDataType(
RTC.TimedDoubleSeq)
"""
フリッパーの角度
- Number: 4
- Semantics: [0]: 右前方フリッパー
[1]: 左前方フリッパー
[2]: 右後方フリッパー
[3]: 左後方フリッパー
- Unit: [rad]
"""
self._FlipperAngle_InIn = OpenRTM_aist.InPort("FlipperAngle_In",
self._d_FlipperAngle_In)
self._d_MotorLoad_In = OpenRTM_aist.instantiateDataType(
RTC.TimedDoubleSeq)
"""
モータの負荷
- Number: 6
- Semantics: [0]: 右メインモータ
[1]: 左メインモータ
[2]: 右前方サブモータ
[3]: 左前方サブモータ
[4]: 右後方サブモータ
[5]: 左後方サブモータ
- Unit: [A]
"""
self._MotorLoad_InIn = OpenRTM_aist.InPort("MotorLoad_In",
self._d_MotorLoad_In)
self._d_Arm1Pose3D_In = OpenRTM_aist.instantiateDataType(
RTC.TimedPose3D)
"""
アーム1の３次元の位置・姿勢
(ロボット座標系)
- Unit: 位置:[m], 姿勢:[rad]
"""
self._Arm1Pose3D_InIn = OpenRTM_aist.InPort("Arm1Pose3D_In",
self._d_Arm1Pose3D_In)
self._d_Arm1Angular_In = OpenRTM_aist.instantiateDataType(
RTC.TimedDoubleSeq)
"""
アーム1の腕の角度
- Semantics: [0]: 1軸目
[1]: 2軸目
[2]: 3軸目
[3]: 4軸目
[4]: 5軸目
[5]: 6軸目
- Unit: [rad]
"""
self._Arm1Angular_InIn = OpenRTM_aist.InPort("Arm1Angular_In",
self._d_Arm1Angular_In)
self._d_Arm1Finger_In = OpenRTM_aist.instantiateDataType(
RTC.TimedLongSeq)
"""
アーム1の指の開閉具合
- Semantics: [0]: 親指
[1]: 差指
[2]: 中指
- Unit: [u] (独自単位)
"""
self._Arm1Finger_InIn = OpenRTM_aist.InPort("Arm1Finger_In",
self._d_Arm1Finger_In)
self._d_Arm1Current_In = OpenRTM_aist.instantiateDataType(
RTC.TimedDoubleSeq)
"""
アーム1のモータ負荷
- Semantics: [0]: 1軸目
[1]: 2軸目
[2]: 3軸目
[3]: 4軸目
[4]: 5軸目
[5]: 6軸目
[6]: 親指
[7]: 差指
[8]: 中指
- Unit: [A]
"""
self._Arm1Current_InIn = OpenRTM_aist.InPort("Arm1Current_In",
self._d_Arm1Current_In)
# initialize of configuration-data.
# <rtc-template block="init_conf_param">
"""
WebSocket 待受ポート番号
- Name: WEBSOCKET_PORT
- DefaultValue: 8080
"""
self._WEBSOCKET_PORT = [8080]
"""
WebSocket 待受アドレス
- Name: WEBSOCKET_HOST
- DefaultValue: 0.0.0.0
"""
self._WEBSOCKET_HOST = ['0.0.0.0']
# </rtc-template>
##
#
# The initialize action (on CREATED->ALIVE transition)
# formaer rtc_init_entry()
#
# @return RTC::ReturnCode_t
#
#
def onInitialize(self):
# Bind variables and configuration variable
self.bindParameter("WEBSOCKET_PORT", self._WEBSOCKET_PORT, "8080")
self.bindParameter("WEBSOCKET_HOST", self._WEBSOCKET_HOST, "0.0.0.0")
# Set InPort buffers
self.addInPort("GCCapacityRatio_In", self._GCCapacityRatio_InIn)
self.addInPort("MainCapacityRatio_In", self._MainCapacityRatio_InIn)
self.addInPort("Mode_In", self._Mode_InIn)
self.addInPort("Pose3D_In", self._Pose3D_InIn)
self.addInPort("Velocity2D_In", self._Velocity2D_InIn)
self.addInPort("FlipperAngle_In", self._FlipperAngle_InIn)
self.addInPort("MotorLoad_In", self._MotorLoad_InIn)
self.addInPort("Arm1Pose3D_In", self._Arm1Pose3D_InIn)
self.addInPort("Arm1Angular_In", self._Arm1Angular_InIn)
self.addInPort("Arm1Finger_In", self._Arm1Finger_InIn)
self.addInPort("Arm1Current_In", self._Arm1Current_InIn)
# Set OutPort buffers
# Set service provider to Ports
# Set service consumers to Ports
# Set CORBA Service Ports
return RTC.RTC_OK
##
#
# The finalize action (on ALIVE->END transition)
# formaer rtc_exiting_entry()
#
# @return RTC::ReturnCode_t
#
def onFinalize(self):
return RTC.RTC_OK
###
##
## The startup action when ExecutionContext startup
## former rtc_starting_entry()
##
## @param ec_id target ExecutionContext Id
##
## @return RTC::ReturnCode_t
##
##
#def onStartup(self, ec_id):
#
# return RTC.RTC_OK
###
##
## The shutdown action when ExecutionContext stop
## former rtc_stopping_entry()
##
## @param ec_id target ExecutionContext Id
##
## @return RTC::ReturnCode_t
##
##
#def onShutdown(self, ec_id):
#
# return RTC.RTC_OK
##
#
# The activated action (Active state entry action)
# former rtc_active_entry()
#
# @param ec_id target ExecutionContext Id
#
# @return RTC::ReturnCode_t
#
#
def onActivated(self, ec_id):
# ビューアーの設定ファイルを生成
config = {
"WEBSOCKET_PORT": self._WEBSOCKET_PORT[0],
}
with open(VIEWER_CONFIG_FILE, mode="w") as f:
json.dump(config,
f,
ensure_ascii=False,
indent=2,
separators=(",", ": "))
#def ws_on_message(client, server, message):
# server.send_message_to_all(message)
# return
# WebSocket サーバーを起動
self.ws_server = WebsocketServer(self._WEBSOCKET_PORT[0],
host=self._WEBSOCKET_HOST[0])
#self.ws_server.set_fn_message_received(ws_on_message)
self.ws_thread = threading.Thread(target=self.ws_server.run_forever)
self.ws_thread.start()
return RTC.RTC_OK
##
#
# The deactivated action (Active state exit action)
# former rtc_active_exit()
#
# @param ec_id target ExecutionContext Id
#
# @return RTC::ReturnCode_t
#
#
def onDeactivated(self, ec_id):
# WebSocket サーバーを終了
self.ws_server.shutdown()
self.ws_server.server_close()
return RTC.RTC_OK
##
#
# The execution action that is invoked periodically
# former rtc_active_do()
#
# @param ec_id target ExecutionContext Id
#
# @return RTC::ReturnCode_t
#
#
def onExecute(self, ec_id):
# 入力ポートに入ってきたデータを WebSocket クライアントに転送する
for port in self._inports:
port_name = port._name
port_type = port._value.__class__.__name__
if port.isNew():
port_in = port.read()
time = port_in.tm.sec + port_in.tm.nsec * (10**-9)
if port_type in [
"TimedDouble", "TimedLongSeq", "TimedDoubleSeq"
]:
data = port_in.data
elif port_type == "TimedPose3D":
position = port_in.data.position
orientation = port_in.data.orientation
data = {
"position": {
"x": position.x,
"y": position.y,
"z": position.z
},
"orientation": {
"r": orientation.r,
"p": orientation.p,
"y": orientation.y
},
}
elif port_type == "TimedVelocity2D":
velocity = port_in.data
data = {
"vx": velocity.vx,
"vy": velocity.vy,
"va": velocity.va
}
js_data = json.dumps({port_name: {"tm": time, "data": data}})
self.ws_server.send_message_to_all(js_data)
#print(js_data)
return RTC.RTC_OK
