def __init__(self):
GPIO.setmode(GPIO.BCM)
self.sensor_x = UltrasonicSensor(18, 24, GPIO)
self.sensor_z = UltrasonicSensor(17, 23, GPIO)
self.arduino = Arduino(arduino_device)
self.server = TcpServer()
self.eye = TargetDetection(True)
self.end_switch = EndSwitch(GPIO)
self.logger = Log.instance('Brain')
self.initial_distance_mm, _ = self.distances(z=False)
def run(self):
try:
server = TcpServer("localhost", 4444)
threading.Thread(target = server.listen).start()
while 1:
raw_input("Press Enter to continue...\n")
server.sendToClient("Server: send to Client")
except KeyboardInterrupt:
print "Shutdown requested...exiting\n"
except Exception:
traceback.print_exc(file=sys.stdout)
sys.exit(0)
def create_(self, ip, port, sockType):
# Check ip and port
server = None
if sockType == socktypes.TCP_SERVER:
server = TcpServer(port, ip)
elif sockType == socktypes.UDP_SERVER:
server = UdpServer(port, ip)
else:
logger.error("sockType not supported")
return 0, ""
_id = server.getId()
if self.serverDict.has_key(_id):
logger.error("server already exists")
del server
return 0, ""
if not server.start_():
return 0, ""
self.serverDict[_id] = server
logger.info("server create ok")
return _id, server.getAddress()
class NetCom:
def __init__(self, receiveCallback):
self.tcpServer = None
self.wsServer = None
self.clients = []
self.clientUID = 0
self.receiveCallback = receiveCallback
def StartTcpServer(self):
self.tcpServer = TcpServer()
self.tcpServer.StartListening(self.newConnection, self.Receive)
print("ready to socket !")
def StopTcpServer(self):
pass
def SendToClient(self, client, msgType, data):
if client.connectionType == ConnectionType.TCP:
client.socket.send(msgType + ":" + data)
elif client.connectionType == ConnectionType.WS:
pass
def SendToAll(self, msgType, data):
for client in self.clients:
self.SendToClient(client, msgType, data)
def Receive(self, client, data):
self.receiveCallback(client, data)
def newConnection(self, connectionType, socket, ip, port, thread):
client = Client(self.clientUID, connectionType, socket, ip, port,
thread)
self.clientUID += 1
self.clients.append(client)
return client
def create_(self, ip, port, sockType):
# Check ip and port
server = None
if sockType == socktypes.TCP_SERVER:
server = TcpServer(port, ip)
elif sockType == socktypes.UDP_SERVER:
server = UdpServer(port, ip)
else:
logger.error("sockType not supported")
return 0, ""
_id = server.getId()
if self.serverDict.has_key(_id):
logger.error("server already exists")
del server
return 0, ""
if not server.start_():
return 0, ""
self.serverDict[_id] = server
logger.info("server create ok")
return _id, server.getAddress()
def __init__(self, port, slaves=[]):
self.slaves = slaves
self.is_slave = True if slaves == [] else False
if not os.path.exists(str(port)):
os.makedirs(str(port))
TcpServer.__init__(self, port)
def StartTcpServer(self):
self.tcpServer = TcpServer()
self.tcpServer.StartListening(self.newConnection, self.Receive)
print("ready to socket !")
from threading import Thread
from TcpServer import TcpServer
from PrintHandler import PrintHandler
from GuiHandler import GuiHandler
from MapAnalysis import MapAnalysis
from RoverHandler import RoverHandler
from RTKHandler import RTKHandler
from MessageHandler import MessageHandler
RoverCon = TcpServer(9001, "Rover")
RoverCon.setAcceptAddress("0.0.0.0")
RoverCon.setMessageHandler(RoverHandler())
RoverThread = Thread()
RTKCon = TcpServer(9002, "RTK")
RTKCon.setAcceptAddress("0.0.0.0")
RTKCon.setMessageHandler(RTKHandler())
RTKThread = Thread()
GuiCon = TcpServer(9003, "GUI")
GuiCon.setAcceptAddress("0.0.0.0")
GuiCon.setMessageHandler(GuiHandler())
GuiThread = Thread()
#parsed_JSON_obj = ()
#parsed_JSON_obj = "[{\"conns\":[2,4],\"coord\":{\"lat\":56.67507440022754,\"long\":12.863477416073408},\"id\":1},{\"conns\":[1,3],\"coord\":{\"lat\":56.67501716123367,\"long\":12.862938208261255},\"id\":2},{\"conns\":[2,4],\"coord\":{\"lat\":56.67521716922783,\"long\":12.862889771317356},\"id\":3},{\"conns\":[3,1],\"coord\":{\"lat\":56.675154300977404,\"long\":12.863227111490545},\"id\":4}]"
rover_position = ""
parsed_JSON_obj = "[{\"conns\":[2,4],\"coord\":{\"lat\":56.66329672972045,\"long\":12.878284881888675},\"id\":1},{\"conns\":[1,3],\"coord\":{\"lat\":56.66324094333677,\"long\":12.87838877294243},\"id\":2},{\"conns\":[2,4],\"coord\":{\"lat\":56.66335448141366,\"long\":12.878455855505564},\"id\":3},{\"conns\":[3,1],\"coord\":{\"lat\":56.66337026074343,\"long\":12.87833033757488},\"id\":4}]"
#rover_position = (56.67515246104728,12.863372800241422)
#map_analysis = ""
#startSignal = False
from Epoll import Epoll from Events import * from Socket import Socket from Handle import Handle from EventLoop import EventLoop from TcpServer import TcpServer loop = EventLoop() server = TcpServer(loop) loop.loop()
def __init__(own, port, slave=[]):
own.slave = slave
own.is_slave = True if slave == [] else False
if not os.path.exists(str(port)):
os.makedirs(str(port))
TcpServer.__init__(own, port)
"Protocol.handleConnected" print "[Protocol.handleConnected]" msg = "Connected Echo Server. Your addr:%s\n" + str(con.addr) con.send(msg, len(msg)) def handleError(self, con, str): "Protocol.handleError" print "[Protocol.handleError]" ,con.addr, str def handleClose(self, con): "Protocol.close" print "[Protocol.close]", con.addr processor = Processor(4) protocol = Protocol() server = TcpServer(protocol, processor) port = 10000 def testListen(): print '=' * 60 print '-' * 20, 'testListen', '-'* 20 server.startAt(port) sleepTime = 6000 while sleepTime > 0: time.sleep(1) sleepTime = sleepTime - 1 print '-' * 20, 'test done', '-' * 20 print '=' * 60
class Brain():
def __init__(self):
GPIO.setmode(GPIO.BCM)
self.sensor_x = UltrasonicSensor(18, 24, GPIO)
self.sensor_z = UltrasonicSensor(17, 23, GPIO)
self.arduino = Arduino(arduino_device)
self.server = TcpServer()
self.eye = TargetDetection(True)
self.end_switch = EndSwitch(GPIO)
self.logger = Log.instance('Brain')
self.initial_distance_mm, _ = self.distances(z=False)
def start(self):
try:
self.logger.info('Silisloth has started its work')
self.drive_to_freight(drive_by_time)
self.pick_up_freight()
self.spot_target_field()
cam_to_target, us_to_pole = self.approach_target_field(critical_mm)
end_us_to_pole = self.calc_end_distance(cam_to_target, us_to_pole)
self.move_freight_over_target(end_us_to_pole)
self.drop_freight()
self.drive_to_pole()
self.arduino.halt()
self.logger.info('Silisloth has finished its work')
except KeyboardInterrupt:
self.logger.info('interrupted by user')
finally:
GPIO.cleanup()
self.logger.info('cleaned up GPIO')
self.server.close()
self.logger.info('closed TCP server')
def drive_to_freight(self, drive_by_time=False):
self.arduino.move_off()
if drive_by_time:
time.sleep(time_to_freight)
else:
self.logger.info('pickup distance: {}'.format(pickup_x_dist_mm))
x = None
while x is None or x > pickup_x_dist_mm:
try:
try_x, _ = self.distances(z=False)
if try_x < 2000: # plausibility check
continue
else:
x = try_x
self.logger.debug('x distance: {}mm'.format(x))
self.logger.debug(
'to freight: {}mm'.format(self.initial_distance_mm -
freight_distance_mm))
except ValueError as e:
self.logger.fatal(e)
def pick_up_freight(self):
try:
x, z = self.distances()
self.logger.info('distance to freight {}mm'.format(z))
self.arduino.pick_up(z)
self.send_coordinates(self.x0ref(x), -1)
self.logger.info('started picking up freight')
except ValueError as e:
self.logger.fatal(e)
self.logger.debug('wait until freight is picked')
x, _ = self.distances(z=False)
while not self.arduino.freight_ready():
self.send_coordinates(self.x0ref(x), -1)
time.sleep(0.5)
self.logger.info('freight was picked, pulling it up')
def spot_target_field(self):
dist_mm = None
while dist_mm is None:
try:
dist_mm = self.eye.distance_to_target()
x, z = self.distances()
z += z_ultrasonic_freight_mm
self.send_coordinates(self.x0ref(x), z)
except Exception as e:
self.logger.debug(e)
msg = 'camera detected target: {}mm away'.format(dist_mm)
self.logger.info(msg)
self.arduino.decelerate()
self.logger.info('decelerate')
def approach_target_field(self, critical_distance):
dist_mm = None
while dist_mm is None or dist_mm >= critical_distance:
try:
dist_mm = self.eye.distance_to_target()
x, z = self.distances(z=False)
self.send_coordinates(self.x0ref(x), -1)
except Exception as e:
self.logger.debug(e)
x_dist_end_mm = None
while x_dist_end_mm is None:
try:
x_dist_end_mm = self.sensor_x.n_median_distance(
measure_n_distances)
except ValueError as e:
self.logger.fatal(e)
return int(dist_mm), int(x_dist_end_mm)
def calc_end_distance(self, cam_to_target, us_to_pole):
end_us_pole_dist_mm = us_to_pole
end_us_pole_dist_mm -= cam_to_target
end_us_pole_dist_mm -= cam_ultrasonic_mm
end_us_pole_dist_mm -= ultrasonic_freight_mm
self.logger.info('cam/target: {}mm'.format(cam_to_target))
self.logger.info('us/pole: {}mm'.format(us_to_pole))
self.logger.info('end us/pole: {}mm'.format(end_us_pole_dist_mm))
return end_us_pole_dist_mm
def move_freight_over_target(self, end):
x = None
while x is None or x > end:
try:
x, z = self.distances(z=False)
self.send_coordinates(self.x0ref(x), -1)
self.logger.info('to pole: {}mm, wanted: {}mm'.format(x, end))
except ValueError as e:
self.logger.fatal(e)
self.logger.info('position over target reached')
def drop_freight(self):
z = None
while z is None:
try:
x, z = self.distances()
except ValueError as e:
self.logger.fata(e)
self.logger.info('drop freight by {}mm'.format(z))
self.arduino.drop_freight(z)
self.logger.debug('wait until freight is released')
_, z = self.distances(x=False)
while not self.arduino.freight_ready():
self.send_coordinates(self.x0ref(x), z)
z -= z_delta_per_second_mm / 2
time.sleep(0.5)
self.logger.info('freight was released, continue')
def drive_to_pole(self):
self.logger.info('approach pole')
x = None
while x is None or x > final_distance_mm:
x, _ = self.distances(z=False)
self.logger.info('distance to pole: {}mm'.format(x))
self.arduino.decelerate()
self.logger.info('wait until end switch is pressed')
while not self.end_switch.is_pressed():
self.send_coordinates(-1, -1)
self.logger.info('end switch was pressed')
def send_coordinates(self, x, z):
self.server.send('x={};z={}\r\n'.format(x, z).encode())
def distances(self, x=True, z=True):
x_dist, z_dist = -1, -1
try:
if x:
x_dist = self.sensor_x.n_median_distance(measure_n_distances)
if z:
z_dist = self.sensor_z.n_median_distance(measure_n_distances)
except ValueError as e:
self.logger.info(e)
return x_dist, z_dist
def x0ref(self, x):
return self.initial_distance_mm - x + x_ultrasonic_start_pole_mm
