def remove_client(self, client):
comms.send(client, comms.EOT)
client_name = self.get_client_name(client)
print 'Server: end connection with %s' % client_name
client.close()
self.inputs.remove(client)
del self.client_map[client_name]
del self.client_name_map[client]
def remove_client(self, client):
comms.send(client, comms.EOT)
client_name = self.get_client_name(client)
print 'Server: end connection with %s' % client_name
client.close()
self.inputs.remove(client)
del self.client_map[client_name]
del self.client_name_map[client]
def doMoveAndTurn():
x = int(raw_input("X: "))
y = int(raw_input("Y: "))
heading = math.degrees(math.atan2(y, x))
distance = math.sqrt(x**2+y**2)
finalHeading = int(raw_input("Final Heading: "))
finalHeading = (finalHeading-heading)%360
cmd = b"M"+struct.pack(">h", heading)+struct.pack(">h", distance)+struct.pack(">h", finalHeading)
comms.send(cmd, robot_id)
def doMoveAndTurn():
x = int(raw_input("X: "))
y = int(raw_input("Y: "))
heading = math.degrees(math.atan2(y, x))
distance = math.sqrt(x**2 + y**2)
finalHeading = int(raw_input("Final Heading: "))
finalHeading = (finalHeading - heading) % 360
cmd = b"M" + struct.pack(">h", heading) + struct.pack(
">h", distance) + struct.pack(">h", finalHeading)
comms.send(cmd, robot_id)
def doOpts():
subcmd = raw_input("[trkm]: ")
if subcmd == "t":
cmd = b"ot"+struct.pack(">B", int(raw_input("t value: ")))
elif subcmd == "r":
cmd = b"or"+struct.pack(">B", int(raw_input("r value: ")))
elif subcmd == "k":
cmd = b"ok"+struct.pack(">B", int(raw_input("k1 value: ")))+struct.pack(">B", int(raw_input("k1 value: ")))
elif subcmd == "m":
cmd = b"om"+struct.pack(">B", int(raw_input("m value: ")))
else:
return
comms.send(cmd, robot_id)
def log_data(sensor_data):
global filename, batch_data
batch_data.append(",".join([str(value) for value in sensor_data]))
if len(batch_data) >= LOG_FREQUENCY:
print(batch_data)
#s = ["".join(x) for x in batch_data]
#print(s)
send(_TCP_CONN, "9K2S", "OBC",
batch_data[0]) #[s.join(x) for x in batch_data])
with open(filename, "a") as f:
for line in batch_data:
f.write(line + "\n")
batch_data = []
def doOpts():
subcmd = raw_input("[trkm]: ")
if subcmd == "t":
cmd = b"ot" + struct.pack(">B", int(raw_input("t value: ")))
elif subcmd == "r":
cmd = b"or" + struct.pack(">B", int(raw_input("r value: ")))
elif subcmd == "k":
cmd = b"ok" + struct.pack(">B", int(
raw_input("k1 value: "))) + struct.pack(
">B", int(raw_input("k1 value: ")))
elif subcmd == "m":
cmd = b"om" + struct.pack(">B", int(raw_input("m value: ")))
else:
return
comms.send(cmd, robot_id)
def send_message(self, client_name, data):
client = self.get_client(client_name)
if client == None:
raise ClientNotFoundException()
self.response_received[client_name] = None
comms.send(client, comms.MSG, data)
timer = threading.Timer(30.0, self.send_message_failed, [client_name])
timer.start()
rate = bwi_tools.WallRate(10)
while self.response_received == None:
rate.sleep()
timer.cancel()
if self.response_received == comms.ACK:
return True
return False
def send_message(self, client_name, data):
client = self.get_client(client_name)
if client == None:
raise ClientNotFoundException()
self.response_received[client_name] = None
comms.send(client, comms.MSG, data)
timer = threading.Timer(30.0, self.send_message_failed, [client_name])
timer.start()
rate = bwi_tools.WallRate(10)
while self.response_received == None:
rate.sleep()
timer.cancel()
if self.response_received == comms.ACK:
return True
return False
def _send_next(self):
msg = self._msg_queue[0]
if msg.destination not in self._robots_in_view:
rospy.logwarn(
"%s attempted to send %s to %s, but robot was not in view" %
(rospy.get_caller_id(), msg.data, msg.destination))
del self._msg_queue[0]
else:
rospy.loginfo("%s attempting to send %s to %s" %
(rospy.get_caller_id(), msg.data, msg.destination))
try:
send(msg.destination, self._ip_addresses[msg.destination],
self.hostname, msg.topic, msg.data)
del self._msg_queue[0]
rospy.loginfo(
"%s successfully sent %s to %s" %
(rospy.get_caller_id(), msg.data, msg.destination))
except: # What's this catching?
# Possibly have a limit for attempts - we don't want to loop forever.
pass
def doData():
filename = raw_input("File to transmit: ")
i2c_freq = int(raw_input("Frequency on i2c: "))
with open(filename, mode='rb') as infile:
data = infile.read(255)
chunk_count = int(math.ceil(len(data) * 1.0 / chunk_size))
# d[1B packet num][1B i2c freq][1B data len]
cmd = b"d\x00" + struct.pack(">B", i2c_freq) + struct.pack(">B", len(data))
comms.send(cmd, robot_id)
# print ":".join("{:02x}".format(ord(c)) for c in cmd)
for i in range(chunk_count):
# time.sleep(1)
# d[1B packet num][1B index of first byte in packet][1B packet len][data]
data_size = chunk_size if i != chunk_count - 1 else len(
data) % chunk_size
first_byte = i * chunk_size
data_str = b"d" + struct.pack(">B", i + 1) + struct.pack(
">B", first_byte) + struct.pack(">B", data_size) + b"".join(
data[first_byte:first_byte + data_size])
# print ":".join("{:02x}".format(ord(c)) for c in data_str)
comms.send(data_str, robot_id)
while (comms.packetlist != []):
time.sleep(1)
# d[1B FF]
cmd = b"d\xff"
comms.send(cmd, robot_id)
def doData():
filename = raw_input("File to transmit: ")
i2c_freq = int(raw_input("Frequency on i2c: "))
with open(filename, mode='rb') as infile:
data = infile.read(255)
chunk_count = int( math.ceil(len(data)*1.0/chunk_size))
# d[1B packet num][1B i2c freq][1B data len]
cmd = b"d\x00" + struct.pack(">B", i2c_freq) + struct.pack(">B", len(data))
comms.send(cmd, robot_id)
# print ":".join("{:02x}".format(ord(c)) for c in cmd)
for i in range(chunk_count):
# time.sleep(1)
# d[1B packet num][1B index of first byte in packet][1B packet len][data]
data_size = chunk_size if i != chunk_count-1 else len(data)%chunk_size
first_byte = i*chunk_size
data_str = b"d" + struct.pack(">B", i+1) + struct.pack(">B", first_byte) + struct.pack(">B", data_size) + b"".join(data[first_byte:first_byte+data_size])
# print ":".join("{:02x}".format(ord(c)) for c in data_str)
comms.send(data_str, robot_id)
while (comms.packetlist != []):
time.sleep(1)
# d[1B FF]
cmd = b"d\xff"
comms.send(cmd, robot_id)
def query(q):
"""
Fetch list of work items given a wiql query
"""
response = comms.send(
'POST',
url=f'https://dev.azure.com/{organisation}/_apis/wit/wiql',
headers=default_headers(),
content={"query": q})
ids = [item['id'] for item in response.json()['workItems']]
work_items = get_work_items_by_id(ids)
return work_items
def ipcDisable(self, listItemId):
comms.send('disable-' + listItemId)
class Server(threading.Thread):
def __init__(self, callback=None, host='', port=12345, max_connections=1):
threading.Thread.__init__(self)
self.server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
self.server.bind((host, port))
print 'Server: listening to port', port, '...'
self.server.listen(max_connections)
self.inputs = [self.server]
#signal.signal(signal.SIGINT, self.signal_handler)
self.callback = callback
self.client_map = {}
self.client_name_map = {}
self.response_received = {}
self.alive = False
self.server_lock = threading.Lock()
def run(self):
rate = bwi_tools.WallRate(20)
self.alive = True
while True:
self.server_lock.acquire()
if not self.alive:
break
try:
inputready,outputready,exceptready = \
select.select(self.inputs, [], [], 0.01)
except select.error, e:
break
except socket.error, e:
break
for s in inputready:
if s == self.server:
# handle the server socket
client, address = self.server.accept()
self.add_client(client, address)
else:
try:
type, data = comms.recv(s)
if type == comms.ENQ:
comms.send(s, comms.ACK)
elif type == comms.EOT:
self.remove_client(s)
elif type == comms.MSG:
success = False
if data and self.callback != None:
success = self.callback(
self.get_client_name(s), data)
if success:
comms.send(s, comms.ACK)
else:
comms.send(s, comms.NACK)
elif type == comms.ACK or type == comms.NACK:
self.response_received[self.get_client_name(
s)] = type
except socket.error, e:
self.remove_client(s)
def ipcWindowed(self):
comms.send('windowed')
def ipcScreenBlack(self):
comms.send('screen-black')
def ipcToggleVisibility(self):
comms.send('toggle-visibility')
def ipcToggleFullscreen(self):
comms.send('toggle-fullscreen')
def ipcPlay(self):
comms.send('play')
def doKick():
distance = int(raw_input("Distance to kick: "))
cmd = b"k"+struct.pack(">h", distance)
comms.send(cmd, robot_id)
# Pack the data with struct
packet = str(sensor.sensor.id) + "," + str(
time.time()) + "," + str(data)
# Put the packed data into a buffer for sending
packetBuffer.append(packet)
# Loop through the sensors and create a thread for each
threads = []
for sensor in sensors:
# Create the thread and give it a sensor as an argument
thread = threading.Thread(target=worker, args=(sensor, ))
# Make it a daemon so it dies on ctrl+c
thread.daemon = True
# Put it in the list of threads
threads.append(thread)
# Start the thread
thread.start()
# Send data to base-station
while True:
try:
data = packetBuffer.pop(0)
comms.send(data)
except:
time.sleep(0.1)
def doKick():
distance = int(raw_input("Distance to kick: "))
cmd = b"k" + struct.pack(">h", distance)
comms.send(cmd, robot_id)
def doTurn():
angle = int(raw_input("Angle: "))
cmd = b"t" + struct.pack(">h", angle)
comms.send(cmd, robot_id)
def ipcEnable(self, listItemId):
comms.send('enable-' + listItemId)
def doMove():
distance = int(raw_input("Distance: "))
cmd = b"m"+struct.pack(">h", distance)
comms.send(cmd, robot_id)
def funComms(self):
comms.send('test')
def ipcNext(self):
comms.send('next')
def ipcTogglePlay(self):
comms.send('toggle-play')
def ipcPrev(self):
comms.send('prev')
def ipcScreenEnable(self):
comms.send('screen-enable')
def ipcRefresh(self):
comms.send('refresh')
def ipcFullscreen(self):
comms.send('fullscreen')
def ipcGet(self, item):
comms.send('get-' + item)
def doTurn():
angle = int(raw_input("Angle: "))
cmd = b"t"+struct.pack(">h", angle)
comms.send(cmd, robot_id)
def ipcTarget(self, target):
comms.send('target-' + target)
def ipcPause(self):
comms.send('pause')
def doMove():
distance = int(raw_input("Distance: "))
cmd = b"m" + struct.pack(">h", distance)
comms.send(cmd, robot_id)
