def ears(self) -> List[int]:
"""Returns a list of the indices of the ears of the polygon"""
pts = np.array(self.pts)
triples = zip(range(self.n), np.roll(range(self.n), -1),
np.roll(range(self.n), -2))
reflex_vertices = {
pts[ear[1]]
for ear in triples if utils.ccw(*pts[list(ear)]) == -1
}
ear_ixs = list()
for i in range(self.n):
ear = [(i - 1) % self.n, i, (i + 1) % self.n]
if utils.ccw(*pts[ear]) != 1:
continue
if self.seg_intersect(pts[[ear[0], ear[2]]]):
continue
closure = Triangle(pts[ear])
if any(closure.contains(v, closed=False) for v in reflex_vertices):
continue
ear_ixs.append(i)
return ear_ixs
def __check_status(self, point, vertex_index):
vertex = self.points[vertex_index]
left_neighbour = self.points[vertex_index - 1]
right_neighbour = self.points[(vertex_index + 1) % len(self.points)]
ccw1 = utils.ccw(point, vertex, left_neighbour)
ccw2 = utils.ccw(point, vertex, right_neighbour)
if ccw1 * ccw2 > 0:
# neighbours lie on the same side
return PreparataHull.PointStatus.TANGENT
elif ccw1 > 0:
return PreparataHull.PointStatus.CONVEX
return PreparataHull.PointStatus.CONCAVE
def contains(self, item, closed=True):
if isinstance(item, Point):
point = item
ccw_01p = utils.ccw(self.pts[0], self.pts[1], point)
ccw_12p = utils.ccw(self.pts[1], self.pts[2], point)
ccw_20p = utils.ccw(self.pts[2], self.pts[0], point)
if closed:
if point in self.pts:
return True
if ccw_01p * ccw_12p * ccw_20p == 0:
# If any is 0, then p is on the edge of triangle
return True
return ccw_01p == ccw_12p == ccw_20p
else:
raise ValueError("item must be instance of [Point]")
def split(A: Point, B: Point,
points: List[Point]) -> Tuple[List[Point], List[Point]]:
"""
Splits list of points into those above the line AB, and those below AB
:param A: on point on line
:param B: second point on line
:param points: list of Points to split
:return: List of points above line, list of points below line
"""
points_up = [p for p in points if ccw(A, B, p) == 1]
points_down = [p for p in points if ccw(A, B, p) == -1]
return points_up, points_down
def is_ear(self, ear: Tuple[int, int, int]) -> bool:
"""Return true if the 3 indices specify the indices of an ear in ccw order."""
pts = np.array(self.pts)
ear = list(ear)
triples = zip(range(self.n), np.roll(range(self.n), -1),
np.roll(range(self.n), -2))
reflex_vertices = {
pts[t[1]]
for t in triples if utils.ccw(*pts[list(t)]) == -1
}
if utils.ccw(*pts[ear]) != 1:
return False
if self.seg_intersect(pts[[ear[0], ear[2]]]):
return False
closure = Triangle(pts[ear])
if any(closure.contains(v, closed=False) for v in reflex_vertices):
return False
return True
def __localize_point(self, left, right, point):
if left == right:
return left - 1, left
mid = left + (right - left) // 2
ccw = utils.ccw(self.edges[mid].source.point, self.edges[mid].to.point,
point)
if ccw > 0:
return self.__localize_point(left, mid, point)
if ccw < 0:
if mid == left:
return mid, mid + 1
return self.__localize_point(mid, right, point)
return mid, mid
def __build_chain(start_point, end_point, points, strategy):
start_index = points.index(start_point)
end_index = points.index(end_point)
seq_len = end_index - start_index
if start_index > end_index:
seq_len = len(points) - start_index + end_index
points_queue = [
points[(start_index + i + 1) % len(points)] for i in range(seq_len)
]
points_queue.reverse()
fake_point = strategy.get_fake_point(start_point)
chain = [fake_point, start_point]
prev_to_vertex = fake_point
while len(points_queue) != 0:
current = points_queue.pop()
if utils.ccw(chain[-2], chain[-1], current) >= 0:
# field 2
while utils.ccw(chain[-2], chain[-1], current) > 0:
chain.pop()
chain.append(current)
else:
if utils.ccw(prev_to_vertex, chain[-1], current) >= 0:
# field 1
while utils.ccw(chain[-1], chain[-2],
points_queue[-1]) >= 0:
points_queue.pop()
else:
if utils.ccw(end_point, chain[-1], current) > 0:
# field 4
while utils.ccw(end_point, chain[-1],
points_queue[-1]) > 0:
points_queue.pop()
else:
# field 3
chain.append(current)
if chain[-1] != start_point:
prev_to_vertex = points[points.index(chain[-1]) - 1]
chain.pop(0)
return chain
# 미발견횟수가 4이상이면 출발 신호 전송
if Count >= 4: # 아무것도 발견 안될때
Count = 0
print('start')
if len(mark) == 0: # 아무것도 발견 안될 때
Count += 1 # 미발견 횟수 증가
for m in mark:
Obj, xmin, xmax, ymin, ymax = get_Object(image, m,
Check) # 발견 물체 정보 추출
if Obj == None:
Count += 1
continue
#print(Obj, " ", ymax)
if Obj == 'red' or Obj == 'green' or Obj == 'stop_sign' or \
(ccw(left, [xmax, ymax]) == 1 and ccw(right, [xmin, ymax]) == -1):
if Check[Obj][0] >= 4: # 발견횟수 4이상
Check[Obj][3] = True # 발견으로 처리
Count = 0
if Obj == 'green': # 초록불 인식
print("restart ", Encode[Obj] + str(ymax))
continue
# 다른 물체 인식
print("stop! ", Encode[Obj] + str(ymax))
break
else:
#print("Object : out of range")
continue
for m in Check: # 탐지 후처리
#미발견 횟수
def obstacleFree(self, v1, v2):
A = [v1.x, v1.y]
B = [v2.x, v2.y]
for obstacle in self.obstacles:
x1, x2, y1, y2 = obstacle.getCorners()
C1 = [x1, y1]
D1 = [x1, y2]
C2 = [x1, y1]
D2 = [x2, y1]
C3 = [x2, y1]
D3 = [x2, y2]
C4 = [x1, y2]
D4 = [x2, y2]
intersect1 = ccw(A, C1, D1) != ccw(B, C1, D1) and ccw(
A, B, C1) != ccw(A, B, D1)
intersect2 = ccw(A, C2, D2) != ccw(B, C2, D2) and ccw(
A, B, C2) != ccw(A, B, D2)
intersect3 = ccw(A, C3, D3) != ccw(B, C3, D3) and ccw(
A, B, C3) != ccw(A, B, D3)
intersect4 = ccw(A, C4, D4) != ccw(B, C4, D4) and ccw(
A, B, C4) != ccw(A, B, D4)
if intersect1 == True or intersect2 == True or intersect3 == True or intersect4 == True:
return False
return True
def _is_valid_ear(ear: List[int], poly):
pts = np.array(poly.pts)
return utils.ccw(*pts[ear]) == 1 and not _seg_intersect_poly(
pts[[ear[0], ear[2]]], poly)
def generateLoop(nBisections, minDistance, direction, ptsIn, ptsOut, ptsObs):
"Generates path from voronoi diagram"
#Inputs
# nBisections - (int) no. times to bisect hexagon sides
# minDistance - (float) closest turtlebot centre can be from obstacles
# direction - (string) 'anticlockwise'/'clockwise': direction to move in
#Outputs
# orderedPoses - (array) nPoses x 3, each row = [x,y,theta] in global coords
# - ordered array of poses forming safest path for robot to move in
# orderedQuivers - (array) nPoses x 4, each row = [x,y,dX,dY] in global coords
# - [dX dY] is vector to next pose. this variable is best used for
# - plotting
#================================================================================
# 1. Adjust environment
#================================================================================
ptsInOriginal = ptsIn
ptsOutOriginal = ptsOut
#create line obstacles: pairs of points
ptsOut = sortPoints(ptsOut)
nPtsOut = len(ptsOut)
nPtsIn = len(ptsIn)
linesObs = []
for i in range(0, nPtsIn):
linesObs.append(np.vstack((ptsIn[i, :], ptsIn[(i + 1) % nPtsIn, :])))
for i in range(0, nPtsOut):
linesObs.append(np.vstack(
(ptsOut[i, :], ptsOut[(i + 1) % nPtsOut, :])))
#bisect ptsOut
for i in range(0, nBisections):
ptsOut = bisectPolygon(ptsOut)
#concatenate points
if ptsObs.size > 0:
pts = np.vstack((ptsIn, ptsOut, ptsObs))
else:
pts = np.vstack((ptsIn, ptsOut))
#================================================================================
# 2. Voronoi diagram
#================================================================================
vor = Voronoi(pts)
nVertices = len(vor.vertices)
for i in range(0, nVertices):
if pointInPolygon(ptsInOriginal, vor.vertices[i, :]):
iInnerVertex = i
vertices = vor.vertices
centrePoint = vertices[iInnerVertex, :]
#form list of lists, which vertices are connected to one another
connections = [[-1]] * nVertices
nConnections = len(vor.ridge_vertices)
for i in range(0, nConnections):
pair = vor.ridge_vertices[i]
v1 = pair[0]
v2 = pair[1]
if (v1 >= 0) & (v2 >= 0):
connections[v1] = addConnection(connections[v1], v2)
connections[v2] = addConnection(connections[v2], v1)
#identify vertices for removal/to be ignored
remove = [iInnerVertex]
connections[iInnerVertex] = []
done = 0
while (not done):
flag = 1
for i in range(0, nVertices):
if (len(connections[i]) == 1) & (i not in remove):
remove.append(i)
connections[i] = []
flag = 0
for i in range(0, nVertices):
connections[i] = [x for x in connections[i] if x not in remove]
if (flag == 1):
done = 1
#================================================================================
# 3. Generate optimum path
#================================================================================
#from connections, generate list of segments
segments = []
nodes = []
for i in range(0, nVertices):
if (len(connections[i]) > 2):
nodes.append(i)
for j in range(0, len(connections[i])):
if (connections[i][j] > i):
segments.append([i, connections[i][j]])
nSegments = len(segments)
segments = sorted(segments, key=itemgetter(0, 1))
distances = np.asarray([float('inf')] * nSegments) #initialise as inf
#no obstacles
if (len(nodes) == 0):
points = np.delete(vertices, remove, axis=0)
orderedPoints = sortPoints(points)
if (direction == 'clockwise'):
orderedPoints = orderedPoints[::-1, :]
nPoses = orderedPoints.shape[0]
orderedPoses = np.hstack((orderedPoints, np.zeros((nPoses, 1))))
orderedQuivers = np.hstack((orderedPoints, np.zeros((nPoses, 2))))
for i in range(0, nPoses):
dY = orderedPoints[(i + 1) % nPoses, 1] - orderedPoints[i, 1]
dX = orderedPoints[(i + 1) % nPoses, 0] - orderedPoints[i, 0]
orderedPoses[i, 2] = np.arctan2(dY, dX)
orderedQuivers[i, 2] = dX
orderedQuivers[i, 3] = dY
return orderedPoses, orderedQuivers
#compute min distance to obstacle for each segment
for i in range(0, nSegments):
#ends of segment i
v1 = vertices[segments[i][0], :]
v2 = vertices[segments[i][1], :]
for j in range(0, len(ptsObs)): #point obstacles
d = pointToLineDistance(v1, v2, ptsObs[j, :])
distances[i] = min(distances[i], d)
for j in range(0, len(linesObs)): #line obstacles
d = lineToLineDistance(linesObs[j][0, :], linesObs[j][1, :], v1,
v2)
distances[i] = min(distances[i], d)
#identify bad segments, remove bad segments from connections
iBadSegments = (distances < minDistance).nonzero()[0]
badSegments = [segments[i] for i in iBadSegments]
for i in range(0, len(badSegments)): #remove from connections
v1 = badSegments[i][0]
v2 = badSegments[i][1]
connections[v1].remove(v2)
connections[v2].remove(v1)
#create branches as lists of points
# start at each node, traverse connections until another node reached -> create branch
branches = []
for i in range(0, len(nodes)):
for j in range(0, len(connections[nodes[i]])):
currentPoint = connections[nodes[i]][j]
branch = [nodes[i], currentPoint]
nodeReached = 0
#keep adding points to branch until reach node or dead end
if (currentPoint in nodes) & (branch[0] < branch[-1]):
branches.append(branch)
else:
while (not nodeReached):
if (len(connections[currentPoint]) == 2):
currentPoint = list(
set(connections[currentPoint]) - set(branch))[0]
branch.append(currentPoint)
if (currentPoint in nodes):
nodeReached = 1
else:
nodeReached = 1
if (currentPoint in nodes) & (branch[0] < branch[-1]):
branches.append(branch)
#switch branch directions based on angles
startNodes = []
endNodes = []
for i in range(0, len(branches)):
# n = 5
# for i in range(n,n+1):
node1 = branches[i][0]
node2 = branches[i][-1]
pStart = vertices[node1, :]
pEnd = vertices[node2, :]
#reverse 2 node branches if clockwise
if (len(branches[i]) == 2):
startToEndCCW = ccw(centrePoint, pStart, pEnd)
if (not startToEndCCW):
branches[i] = branches[i][::-1]
#reverse > 2 node branches if clockwise
elif (len(branches[i]) > 2):
middle = branches[i][len(branches[i]) / 2]
pMiddle = vertices[middle, :]
startToMiddleCCW = ccw(centrePoint, pStart, pMiddle)
middleToEndCCW = ccw(centrePoint, pMiddle, pEnd)
if not (startToMiddleCCW and middleToEndCCW):
branches[i] = branches[i][::-1]
#reverse all if clockwise desired
if (direction == 'clockwise'):
branches[i] = branches[i][::-1]
#store start and end nodes
startNodes.append(branches[i][0])
endNodes.append(branches[i][-1])
#sort by endpoints
#branches = sorted(branches,key=itemgetter(0,-1))
#remove dead ends (nodes only appearing in endNodes)
deadEnds = list(set(endNodes) - set(startNodes))
while (len(deadEnds) > 0): #repeat until no dead ends
nodes = [x for x in nodes if x not in deadEnds]
deadBranches = []
for i in range(0, len(branches)):
if endNodes[i] in deadEnds:
deadBranches.append(i)
startNodes = [
i for j, i in enumerate(startNodes) if j not in deadBranches
]
endNodes = [i for j, i in enumerate(endNodes) if j not in deadBranches]
branches = [i for j, i in enumerate(branches) if j not in deadBranches]
deadEnds = list(set(endNodes) - set(startNodes))
if (len(branches) == 0):
print('Error: all branches removed')
#get min distance to obstacle of each branch
if (len(branches) > 0):
branchDistances = np.asarray([float('inf')] *
len(branches)) #initialise as inf
for i in range(0, len(branches)):
for j in range(0, len(branches[i]) - 1):
p1 = branches[i][j]
p2 = branches[i][j + 1]
#find segment connected p1,p2
iSegment = segments.index(sorted([p1, p2], key=int))
branchDistances[i] = min(branchDistances[i],
distances[iSegment])
#create ordered list of branches
currentNode = nodes[0]
nodeSequence = [currentNode]
branchSequence = []
done = 0
while (not done):
#find currentNode in startNodes
currentBranches = [
i for i, x in enumerate(startNodes) if x == currentNode
]
currentDistances = branchDistances[currentBranches]
branchIndex = currentBranches[np.argmax(currentDistances)]
branchSequence.append(branchIndex)
currentNode = branches[branchIndex][-1]
if (nodeSequence[0] == currentNode):
done = 1
elif (len(nodeSequence) > 2 * len(nodes)):
done = 1
print('Error: could not complete cycle')
else:
nodeSequence.append(currentNode)
# branches sequence -> ordered points -> ordered poses
pointSequence = []
for i in range(0, len(branchSequence)):
pointSequence = pointSequence + branches[branchSequence[i]]
del pointSequence[-1] #remove duplicate
orderedPoints = vertices[pointSequence, :]
nPoses = orderedPoints.shape[0]
orderedPoses = np.hstack((orderedPoints, np.zeros((nPoses, 1))))
orderedQuivers = np.hstack((orderedPoints, np.zeros((nPoses, 2))))
for i in range(0, nPoses):
dY = orderedPoints[(i + 1) % nPoses, 1] - orderedPoints[i, 1]
dX = orderedPoints[(i + 1) % nPoses, 0] - orderedPoints[i, 0]
orderedPoses[i, 2] = np.arctan2(dY, dX)
orderedQuivers[i, 2] = dX
orderedQuivers[i, 3] = dY
return orderedPoses, orderedQuivers
