def detectCol():
stt = fileHandler.loadJsonFromFile('state.json')
sps = fileHandler.loadJsonFromFile('ships.json')
mX = float(stt["lng"])
mY = float(stt["lat"])
mvX = float(stt["vx"])
mvY = float(stt["vy"])
colShips = []
for ship in sps["ships"]:
x = float(ship["lon"])
y = float(ship["lat"])
hed = float(ship["hed"]) * math.pi / 180
V = float(ship["sog"])
vx = V * math.cos(hed)
vy = V * math.sin(hed)
rx = x - mX
ry = y - mY
rvx = vx - mvX
rvy = vy - mvY
a = rvy / rvx
b = ry - a * rx
d = abs(b) / (a**2 + 1)**0.5
if d <= dMin:
obst = Rectangle(maxes=[x + dX, y + dY], mins=[x - dX, y - dY])
colShips.append([obst, vx, vy])
return colShips
def colAvoid(colShips, Wi, i):
stt = fileHandler.loadJsonFromFile('state.json')
pts = fileHandler.loadJsonFromFile('points.json')
W = [pts["points"][Wi]["x"], pts["points"][Wi]["y"]]
mX = float(stt["lng"])
mY = float(stt["lat"])
pt1 = [mX, mY]
pt2 = W
dx = pt2[0] - pt1[0]
dy = pt2[1] - pt1[1]
D = (dx**2 + dy**2)**0.5
try:
rx = dx / D * dR
ry = dy / D * dR
except:
rx = 0
ry = 0
x1 = pt2[0] - rx
y1 = pt2[1] - ry
stop = [x1, y1]
evoalg.start = pt1
evoalg.stop = stop
evoalg.obsts = []
evoalg.movObsts = colShips
x, y = evoalg.evolutionAlgorithm(colShips.__len__())
stop = np.array(stop)
lne = line()
lne = lne[i:]
end = len(lne) - 1
for pt in list(reversed(lne)):
npt = np.array(pt)
dist = np.linalg.norm(stop - npt)
if dist < dE:
break
else:
end -= 1
if end < i + 1:
end = i + 1
lne = line()
lne = np.array(lne)
nlne = np.stack((x, y), axis=-1)
l1 = lne[0:i + 1]
l2 = lne[end:]
fline = np.concatenate((l1, nlne, l2), axis=0)
fileHandler.saveLineToFile(fline)
def controlUpdate(self, hdg, lat, lon):
self.myPos = [lat, lon]
if distanceMeters.getDistanceMeters(lat, lon, self.myPt[0], self.myPt[1]) < self.thresh:
self.i = self.i+1
if self.i < self.points[0].__len__():
self.myPt = [self.points[0][self.i], self.points[1][self.i]]
else:
stt = fileHandler.loadJsonFromFile('state.json')
stt["go"] = str(0)
self.setServos(0, 0)
return
if distanceMeters.getDistanceMeters(lat, lon, self.myWP[0], self.myWP[1]) < self.Wthresh:
self.Wi = self.Wi+1
self.myWP = [self.waypoints[0][self.Wi], self.waypoints[1][self.Wi]]
zadane = self.getDesiredAngle(self.myPos, self.myPt)
self.pid.SetPoint = zadane
self.pid.update(hdg)
output = self.pid.output
wejscie = output
if wejscie > math.pi/2:
wejscie = math.pi/2
if wejscie < -math.pi/2:
wejscie = -math.pi/2
if wejscie > 0:
Fl = 1000-wejscie*1000/math.pi
Fr = 1000
else:
Fr = 1000+wejscie*1000/math.pi
Fl = 1000
self.setServos(Fl, Fr)
def waypoints():
lne = fileHandler.loadJsonFromFile('points.json')
pts = []
for pt in lne["points"]:
pts.append([float(pt['lng']), float(pt['lat'])])
pts = np.array(pts)
return pts
def line():
lne = fileHandler.loadJsonFromFile('line.json')
pts = []
for pt in lne["line"]:
pts.append([float(pt['x']), float(pt['y'])])
pts = np.array(pts)
return pts
def listener(self, name, message):
stt = fileHandler.loadJsonFromFile('state.json')
stt["lng"] = message.lon
stt["lat"] = message.lat
stt["heading"] = message.hdg
stt["vx"] = message.vx
stt["vy"] = message.vy
fileHandler.saveStateToFile(stt)
def do_GET(self):
parsed_path = urlparse(self.path)
path = parsed_path.path
self._set_headers()
if 'points' in path:
self.pts = fileHandler.loadJsonFromFile('points.json')
dataToWrite = json.dumps(self.pts).encode('ascii')
elif 'line' in path:
self.lne = fileHandler.loadJsonFromFile('line.json')
dataToWrite = json.dumps(self.lne).encode('ascii')
elif 'ships' in path:
self.sps = fileHandler.loadJsonFromFile('ships.json')
dataToWrite = json.dumps(self.sps).encode('ascii')
elif 'state' in path:
self.stt = fileHandler.loadJsonFromFile('state.json')
dataToWrite = json.dumps(self.stt).encode('ascii')
else:
dataToWrite = "{}".encode('ascii')
self.wfile.write(dataToWrite)
def run(self):
while True:
stt = fileHandler.loadJsonFromFile('state.json')
go = int(stt["go"])
arm = int(stt["arm"])
if self.mavlinker.amArmed:
if go == 1:
if self.mavlinker.myPt == [0, 0]:
pts = collision_avoider.line()
Wpts = collision_avoider.waypoints()
self.mavlinker.points = pts
self.mavlinker.waypoints = Wpts
self.mavlinker.myPt = [
self.mavlinker.points[0][0],
self.mavlinker.points[1][0]
]
self.mavlinker.myWP = [
self.mavlinker.points[0][0],
self.mavlinker.points[1][0]
]
self.mavlinker.i = 0
self.mavlinker.Wi = 0
self.mavlinker.controlUpdate(float(stt["hdg"]),
float(stt['lat']),
float(stt["lng"]))
else:
if not self.mavlinker.myPt == [0, 0]:
self.mavlinker.myPt = [0, 0]
self.mavlinker.myWP = [0, 0]
self.mavlinker.points = []
self.mavlinker.waypoints = []
self.itercnt += 1
if self.itercnt > 10:
obst = collision_avoider.detectCol()
self.itercnt = 0
if obst:
self.mavlinker.setServos(0, 0)
collision_avoider.colAvoid(obst, self.mavlinker.Wi,
self.mavlinker.i)
if arm == 1 and self.mavlinker.amArmed:
self.mavlinker.arm()
self.mavlinker.amArmed = True
time.sleep(T)
def do_POST(self):
parsed_path = urlparse(self.path)
path = parsed_path.path
if 'operator' in path:
content_length = int(
self.headers['Content-Length']) # <--- Gets the size of data
post_data = self.rfile.read(
content_length) # <--- Gets the data itself
command = json.loads(post_data.decode('utf-8'))
if command["command"] == "add":
point = command["data"]
self.pts["points"].append(point)
if command["command"] == "delete":
point = command["data"]
for el in self.pts["points"]:
if el["id"] == int(point):
del self.pts["points"][self.pts["points"].index(el)]
if command["command"] == "update":
point = json.loads(command["data"])
for el in self.pts["points"]:
if el["id"] == int(point["id"]):
self.pts["points"][self.pts["points"].index(
el)] = point
fileHandler.savePtsToFile(self.pts)
self.lne = ptsToLine(self.pts)
self._set_headers()
dataToWrite = json.dumps(self.lne).encode('ascii')
self.wfile.write(dataToWrite)
if 'ais' in path:
content_length = int(
self.headers['Content-Length']) # <--- Gets the size of data
post_data = self.rfile.read(
content_length) # <--- Gets the data itself
command = json.loads(post_data.decode('utf-8'))
if command["command"] == "add":
ship = json.loads(command["data"])
self.sps["ships"].append(ship)
if command["command"] == "delete":
ship = command["data"]
for el in self.sps["ships"]:
if el["id"] == int(ship):
del self.sps["ships"][self.sps["ships"].index(el)]
if command["command"] == "update":
ship = json.loads(command["data"])
for el in self.sps["ships"]:
if el["id"] == int(ship["id"]):
self.sps["ships"][self.sps["ships"].index(el)] = ship
fileHandler.savePtsToFile(self.pts)
self.lne = ptsToLine(self.pts)
self._set_headers()
dataToWrite = json.dumps(self.pts).encode('ascii')
self.wfile.write(dataToWrite)
if 'start' in path:
self.stt = fileHandler.loadJsonFromFile('state.json')
if int(self.stt["arm"]) == 1:
self.stt["go"] = 1
fileHandler.saveStateToFile(self.stt)
self._set_headers()
dataToWrite = json.dumps(self.lne).encode('ascii')
self.wfile.write(dataToWrite)
if 'stop' in path:
self.stt = fileHandler.loadJsonFromFile('state.json')
self.stt["go"] = 0
fileHandler.saveStateToFile(self.stt)
self._set_headers()
dataToWrite = json.dumps(self.lne).encode('ascii')
self.wfile.write(dataToWrite)
if 'arm' in path:
self.stt = fileHandler.loadJsonFromFile('state.json')
self.stt["arm"] = 1
fileHandler.saveStateToFile(self.stt)
self._set_headers()
dataToWrite = json.dumps(self.lne).encode('ascii')
self.wfile.write(dataToWrite)
def listener(self, name, message):
stt = fileHandler.loadJsonFromFile('state.json')
stt["dph"] = message.distance
fileHandler.saveStateToFile(stt)