def avoid_obstacle(endPoint):
if endPoint is not None:
minDist = 0
closestToBall_index = 0
robot = singleton.Singleton.robot
robot_center = (robot.centrumX, robot.centrumY)
waypoint_list = singleton.Singleton.way_points
safe_points = singleton.Singleton.safe_points
safe_point_dist_to_robot = []
direct_path_line = LineString([(robot_center[0], robot_center[1]),
(endPoint.x, endPoint.y)])
if lines.areLineTouchingObstacleSquare(direct_path_line):
for i in range(len(safe_points)):
dist = calc_pix_dist(endPoint.x, endPoint.y, safe_points[i].x,
safe_points[i].y)
if minDist == 0:
minDist = dist
elif dist < minDist:
minDist = dist
closestToBall_index = i
for i in range(len(safe_points)):
dist2 = calc_pix_dist(robot.centrumX, robot.centrumY,
safe_points[i].x, safe_points[i].y)
index_and_dist = (dist2, i)
safe_point_dist_to_robot.append(index_and_dist)
safe_point_dist_to_robot.sort()
for i in range(2):
if i == 0:
safe_point_index1 = safe_point_dist_to_robot[i][1]
dist3 = calc_pix_dist(safe_points[safe_point_index1].x,
safe_points[safe_point_index1].y,
safe_points[closestToBall_index].x,
safe_points[closestToBall_index].y)
if i == 1:
safe_point_index = safe_point_dist_to_robot[i][1]
dist4 = calc_pix_dist(safe_points[safe_point_index].x,
safe_points[safe_point_index].y,
safe_points[closestToBall_index].x,
safe_points[closestToBall_index].y)
if dist3 < dist4:
first_waypoint = point.Point(
safe_points[safe_point_index1].x,
safe_points[safe_point_index1].y)
else:
first_waypoint = point.Point(
safe_points[safe_point_index].x,
safe_points[safe_point_index].y)
waypoint_list.append(first_waypoint)
direct_path_line = LineString([(first_waypoint.x,
first_waypoint.y),
(endPoint.x, endPoint.y)])
if lines.areLineTouchingObstacleSquare(direct_path_line):
second_waypoint = point.Point(
safe_points[closestToBall_index].x,
safe_points[closestToBall_index].y)
waypoint_list.append(second_waypoint)
def ball_cen_correction(cam_cen, ball_cen):
x_correction = round((cam_cen.x - ball_cen.x) / cam_ball_scale)
y_correction = round((cam_cen.y - ball_cen.y) / cam_ball_scale)
px = round(ball_cen.x + x_correction / 2)
py = round(ball_cen.y + y_correction / 2)
p = point.Point(px, py)
return p
def obstacle_cen_correction(cam_cen, obstacle_cen):
x_correction = round((cam_cen.x - obstacle_cen.x) / cam_obstacle_scale)
y_correction = round((cam_cen.y - obstacle_cen.y) / cam_obstacle_scale)
px = obstacle_cen.x + x_correction
py = obstacle_cen.y + y_correction
p = point.Point(px, py)
return p
def goal_cen_correction(cam_cen, goal_cen):
x_correction = round((cam_cen.x - goal_cen.x) / cam_goal_scale)
y_correction = round((cam_cen.y - goal_cen.y) / cam_goal_scale)
px = goal_cen.x + x_correction
py = goal_cen.y + y_correction
p = point.Point(px, py)
return p
def point_correction(cam_cen, bl_point):
x_correction = round((cam_cen.x - bl_point.x) / cam_robot_scale)
y_correction = round((cam_cen.y - bl_point.y) / cam_robot_scale)
px = bl_point.x + x_correction
py = bl_point.y + y_correction
p = point.Point(px, py)
return p
def getWpGoal(direction):
track = singleton.Singleton.track
#False is left goal
if direction == False:
if track.bigGoal.x > track.smallGoal.x:
wpGoal = point.Point(track.bigGoal.x - round(track.pixelConversion*35), track.bigGoal.y)
elif track.bigGoal.x < track.smallGoal.x:
wpGoal = (track.bigGoal.x + round(track.pixelConversion*35), track.bigGoal.y)
# True is right goal
elif direction == True:
if track.bigGoal.x > track.smallGoal.x:
wpGoal = point.Point(track.smallGoal.x + round(track.pixelConversion*35), track.smallGoal.y)
elif track.bigGoal.x < track.smallGoal.x:
wpGoal = point.Point(track.smallGoal.x - round(track.pixelConversion*35), track.smallGoal.y)
return wpGoal
def calculateGoals(corner1, corner2):
goalMidpoint = point.Point(round((corner1.x + corner2.x) / 2),
round(((corner1.y + corner2.y) / 2) - 8))
# goalMidpoint.x = (corner1.x + corner2.x)/2
# goalMidpoint.y = (corner1.y + corner2.y)/2
return goalMidpoint
def getBalls(img):
global tempBall
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)
gray = cv2.medianBlur(gray, 3)
rows = gray.shape[1]
# Original:
circles = cv2.HoughCircles(gray,
cv2.HOUGH_GRADIENT,
1,
10,
param1=500,
param2=26,
minRadius=1,
maxRadius=20)
# Mere følsom:
# circles = cv2.HoughCircles(gray, cv2.HOUGH_GRADIENT, 1, 10, param1=500, param2=20, minRadius=1, maxRadius=20)
# circles = cv2.HoughCircles(gray, cv2.HOUGH_GRADIENT, 1, 10, param1=500, param2=15, minRadius=1, maxRadius=20)
# print(circles)
if circles is not None:
circles = np.uint16(np.around(circles))
tempBall = []
for i in circles[0, :]:
center = (i[0], i[1])
# circle center
# cv2.circle(img, center, 1, (0, 100, 100), 5)
# # circle outline
# radius = i[2]
# cv2.circle(img, center, radius, (255, 0, 255), 3)
singleBall = ball.Ball(i[0], i[1], i[2])
x_val = np.amax(img, axis=0)
y_val = np.amax(img, axis=1)
x_val = round(len(x_val) / 2)
y_val = round(len(y_val) / 2)
camera_center = point.Point(x_val, y_val)
corrected_ball_center = correction.ball_cen_correction(
camera_center, singleBall)
singleBall.x = int(corrected_ball_center.x)
singleBall.y = int(corrected_ball_center.y)
# print("correctedball_X: " + str(corrected_ball_center.x))
# print("correctedball_Y: " + str(corrected_ball_center.y))
tempBall.append(singleBall)
else:
tempBall.clear()
return tempBall
return tempBall
def getRobot(img):
# Capture frame-by-frame
tempRobot = robot.Robot
blurred = cv2.GaussianBlur(img, (5, 5), 0)
hsv = cv2.cvtColor(blurred, cv2.COLOR_BGR2HSV)
boundaries = [([27, 50, 20], [35, 255, 255])]
boundaries1 = [
# Dag kørsel:
([120, 40, 20], [175, 255, 255])
#Nat kørsel:
# ([140, 40, 10], [190, 255, 255])
]
# roed: ([17, 15, 100], [50, 56, 200])
# blaa: ([86, 31, 4], [220, 88, 50]),
# gul: ([25, 146, 190], [62, 174, 250])
# graa: ([103, 86, 65], [145, 133, 128])
# loop over the boundaries
for (lower, upper) in boundaries:
# create NumPy arrays from the boundaries
lower = np.array(lower, dtype="uint8")
upper = np.array(upper, dtype="uint8")
# find the colors within the specified boundaries and apply
mask = cv2.inRange(hsv, lower, upper)
for (lower, upper) in boundaries1:
# create NumPy arrays from the boundaries
lower1 = np.array(lower, dtype="uint8")
upper1 = np.array(upper, dtype="uint8")
mask1 = cv2.inRange(hsv, lower1, upper1)
# tempRobot = robot.Robot
# Finding the biggest contour to find robot
contours, _ = cv2.findContours(mask1, 1, 1)
max_area = 0
best_cnt = 0
for cnt in contours:
area = cv2.contourArea(cnt)
if area > max_area:
max_area = area
best_cnt = cnt
# loop over the contours
cont, _ = cv2.findContours(mask, 1, 1)
bl_max_area = 0
bl_best_cnt = 0
for c in cont:
# compute the center of the contour
bl_area = cv2.contourArea(c)
if bl_area > bl_max_area:
bl_max_area = bl_area
bl_best_cnt = c
B = cv2.moments(bl_best_cnt)
if B["m00"] != 0:
cX = round(B["m10"] / B["m00"])
cY = round(B["m01"] / B["m00"])
else:
# set values as what you need in the situation
cX, cY = 0, 0
#
# # draw the contour and center of the shape on the image
# Pixel correction
x_val = np.amax(img, axis=0)
y_val = np.amax(img, axis=1)
x_val = round(len(x_val) / 2)
y_val = round(len(y_val) / 2)
camera_center = point.Point(x_val, y_val)
bl = point.Point(cX, cY)
blSquare_corrected = point_correction(camera_center, bl)
# tempRobot.blSquareX = cX
# tempRobot.blSquareY = cY
tempRobot.blSquareX = blSquare_corrected.x
tempRobot.blSquareY = blSquare_corrected.y
# Center of robot
if best_cnt is not None:
M = cv2.moments(best_cnt)
cx, cy = round(M['m10'] / M['m00']), int(M['m01'] / M['m00'])
rect = cv2.minAreaRect(best_cnt)
# Rotating box
box = cv2.boxPoints(rect)
box = np.int0(box)
robot_center = point.Point(cx, cy)
center_corrected = point_correction(camera_center, robot_center)
# tempRobot.centrumX = cx
# tempRobot.centrumY = cy
tempRobot.centrumX = center_corrected.x
tempRobot.centrumY = center_corrected.y
tempRobot.box = box
scale_percent = 30 # percent of original size
width = int(mask1.shape[1] * scale_percent / 100)
height = int(mask1.shape[0] * scale_percent / 100)
dim = (width, height)
# resize image
resized = cv2.resize(mask1, dim, interpolation=cv2.INTER_AREA)
scale_percent = 30 # percent of original size
width = int(mask.shape[1] * scale_percent / 100)
height = int(mask.shape[0] * scale_percent / 100)
dim = (width, height)
# resize image
resized1 = cv2.resize(mask, dim, interpolation=cv2.INTER_AREA)
cv2.imshow("maskPurple", resized)
cv2.imshow("maskYellow", resized1)
# print("\n")
return tempRobot
def getTrack(frame):
tempTrack = track.Track
corner_boundaries = [
#day
([37, 50, 20], [96, 255, 255])
# ([37, 50, 20], [96, 255, 255])
# ([86, 0, 0], [255, 0, 0])
]
blurred_frame = cv2.GaussianBlur(frame, (5, 5), 0)
hsv = cv2.cvtColor(blurred_frame, cv2.COLOR_BGR2HSV)
points = []
# loop over the boundaries
for (lower, upper) in corner_boundaries:
# create NumPy arrays from the boundaries
lower = np.array(lower, dtype="uint8")
upper = np.array(upper, dtype="uint8")
# find the colors within the specified boundaries and apply
# the mask
mask = cv2.inRange(hsv, lower, upper)
# coutput = cv2.bitwise_and(hsv, hsv, mask=mask)
# find contours in the thresholded image
cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
cnts = imutils.grab_contours(cnts)
areaArray = []
for i, c in enumerate(cnts):
area = cv2.contourArea(c)
areaArray.append(area)
# first sort the array by area
sorteddata = sorted(zip(areaArray, cnts), key=lambda x: x[0], reverse=True)
biggest4 = [
sorteddata[0][1], sorteddata[1][1], sorteddata[2][1], sorteddata[3][1]
]
count = 0
# loop over the contours
for c in biggest4:
# compute the center of the contour
M = cv2.moments(c)
count = count + 1
# print(str(count))
if M["m00"] != 0:
cX = round(M["m10"] / M["m00"])
cY = round(M["m01"] / M["m00"])
# print("count: " + str(count) + " cX: " + str(cX) + " cY: " + str(cY))
# draw the contour and center of the shape on the image
# cv2.drawContours(frame, [c], -1, (0, 255, 0), 2)
# cv2.circle(frame, (cX, cY), 3, (255, 255, 255), -1)
# cv2.putText(frame, "center", (cX - 20, cY - 20),
# cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2)
# print("X: " + str(cX))
# print("Y: " + str(cY))
# Add points to array of points
points.append([cX, cY])
if len(points) == 4:
# Set corners on track object
sortedPoints = sort(points)
tempTrack.topLeftCorner = corner.Corner(sortedPoints[0][0],
sortedPoints[0][1])
tempTrack.bottomLeftCorner = corner.Corner(sortedPoints[1][0],
sortedPoints[1][1])
tempTrack.topRightCorner = corner.Corner(sortedPoints[2][0],
sortedPoints[2][1])
tempTrack.bottomRightCorner = corner.Corner(sortedPoints[3][0],
sortedPoints[3][1])
# Calculate goals
# Pixel correction
x_val = np.amax(frame, axis=0)
y_val = np.amax(frame, axis=1)
x_val = round(len(x_val) / 2)
y_val = round(len(y_val) / 2)
camera_center = point.Point(x_val, y_val)
goal_small = calculateGoals(tempTrack.topLeftCorner,
tempTrack.bottomLeftCorner)
goal_big = calculateGoals(tempTrack.topRightCorner,
tempTrack.bottomRightCorner)
goal_big_corrected = goal_cen_correction(camera_center, goal_big)
goal_small_corrected = goal_cen_correction(camera_center, goal_small)
tempTrack.smallGoal = goal_small_corrected
tempTrack.bigGoal = goal_big_corrected
# tempTrack.smallGoal = calculateGoals(tempTrack.topLeftCorner, tempTrack.bottomLeftCorner)
# tempTrack.bigGoal = calculateGoals(tempTrack.topRightCorner, tempTrack.bottomRightCorner)
# Calculate pixel/cm conversion
LenghtX = tempTrack.topRightCorner.x - tempTrack.topLeftCorner.x
LenghtY = tempTrack.topRightCorner.y - tempTrack.topLeftCorner.y
HeightX = tempTrack.topRightCorner.x - tempTrack.bottomLeftCorner.x
HeightY = tempTrack.topRightCorner.y - tempTrack.bottomLeftCorner.y
trackLenght = math.sqrt(pow(LenghtX, 2) + pow(LenghtY, 2))
# trackHeight = math.sqrt(pow(HeightX, 2) + pow(HeightY, 2))
tempTrack.pixelConversion = trackLenght / 169
# cv2.imshow("mask", mask)
return tempTrack
def findBestBall(balls):
robot = singleton.Singleton.robot
track = singleton.Singleton.track
minDist = 0
dist = 0
maxNumberOfTries = 5
numberOfBallsLeft = len(singleton.Singleton.balls)
danger = track.pixelConversion * 5
ball_dist = []
cornerSafePointX = track.pixelConversion * 3
cornerSafePointY = track.pixelConversion * 12
sideSafePoint = track.pixelConversion * 7
tempBall = Ball
print("Choose ball")
# return ball
if numberOfBallsLeft == 0:
return None
else:
for ball in balls:
dist = math.sqrt(
pow(robot.centrumX - ball.x, 2) +
pow(robot.centrumY - ball.y, 2))
ball_point = point.Point(ball.x, ball.y)
ball_dist.append((dist, ball_point))
if minDist == 0:
minDist = dist
tempBall.x = ball.x
tempBall.y = ball.y
elif dist < minDist:
minDist = dist
tempBall.x = ball.x
tempBall.y = ball.y
ball_dist.sort()
chosen_ball = ball_dist[0][1]
if is_ball_in_obstacle(chosen_ball):
balls_in_obstacle = 0
# ball_dist.pop(0)
for i in range(len(ball_dist) - 1, -1, -1):
if is_ball_in_obstacle(ball_dist[i][1]):
ball_dist.pop(i)
balls_in_obstacle += 1
if numberOfBallsLeft == balls_in_obstacle:
return tempBall
else:
chosen_ball = ball_dist[0][1]
tempBall.x = chosen_ball.x
tempBall.y = chosen_ball.y
return tempBall
# elif singleton.Singleton.firstBall:
# chosen_ball = ball_dist[1][1]
# tempBall.x = chosen_ball.x
# tempBall.y = chosen_ball.y
# return tempBall
elif ball_dist[0][0] < track.pixelConversion * 15:
for i in range(len(ball_dist)):
if ball_dist[i][0] > track.pixelConversion * 15:
chosen_ball = ball_dist[i][1]
tempBall.x = chosen_ball.x
tempBall.y = chosen_ball.y
return tempBall
chosen_ball = ball_dist[0][1]
tempBall.x = chosen_ball.x
tempBall.y = chosen_ball.y
return tempBall
else:
tempBall.x = chosen_ball.x
tempBall.y = chosen_ball.y
return tempBall
def waypoints(endPoint):
minDist = 0
closestToBall_index = 0
track = singleton.Singleton.track
danger = round(track.pixelConversion * 15)
cornerSafePointX = round(track.pixelConversion * 20)
cornerSafePointY = round(track.pixelConversion * 45)
sideSafePoint = round(track.pixelConversion * 25)
line_length = round(track.pixelConversion * 23)
robot = singleton.Singleton.robot
robot_center = (robot.centrumX, robot.centrumY)
obstacle = singleton.Singleton.obstacle
singleton.Singleton.way_points.clear()
waypoint_list = singleton.Singleton.way_points
safe_points = singleton.Singleton.safe_points
safe_point_dist_to_robot = []
obstacleDanger = round(track.pixelConversion * 12)
# for i in range(len(safe_points)):
# dist = calc_pix_dist(endPoint.x, endPoint.y, safe_points[i].x, safe_points[i].y)
# if minDist == 0:
# minDist = dist
# elif dist < minDist:
# minDist = dist
# closestToBall_index = i
# for i in range(len(safe_points)):
# dist2 = calc_pix_dist(robot.centrumX, robot.centrumY, safe_points[i].x, safe_points[i].y)
# index_and_dist = (dist2, i)
# safe_point_dist_to_robot.append(index_and_dist)
# safe_point_dist_to_robot.sort()
# for i in range(2):
# if i == 0:
# safe_point_index1 = safe_point_dist_to_robot[i][1]
# dist3 = calc_pix_dist(safe_points[safe_point_index1].x, safe_points[safe_point_index1].y, projected_point_x,
# projected_point_y)
# if i == 1:
# safe_point_index = safe_point_dist_to_robot[i][1]
# dist4 = calc_pix_dist(safe_points[safe_point_index].x, safe_points[safe_point_index].y, projected_point_x,
# projected_point_y)
# if dist3 < dist4:
# first_waypoint = safe_points[safe_point_index1]
# else:
# first_waypoint = safe_points[safe_point_index]
if endPoint is not None:
direct_path_line = LineString([(robot_center[0], robot_center[1]),
(endPoint.x, endPoint.y)])
# If the ball is inside the bounding square of the obstacle
if obstacle.square_bottom_right_corner.x >= endPoint.x >= obstacle.square_top_left_corner.x and obstacle.square_bottom_right_corner.y >= endPoint.y >= obstacle.square_top_left_corner.y:
print("Ball is inside the obstacle!")
cm10_pix = round(10 * track.pixelConversion)
center_to_ball_dist = calc_pix_dist(obstacle.center_x,
obstacle.center_y, endPoint.x,
endPoint.y)
scale = line_length / center_to_ball_dist
projected_point_x = round((
(endPoint.x - obstacle.center_x) * scale) + obstacle.center_x)
projected_point_y = round((
(endPoint.y - obstacle.center_y) * scale) + obstacle.center_y)
projected_point = point.Point(projected_point_x, projected_point_y)
for i in range(len(safe_points)):
dist = calc_pix_dist(endPoint.x, endPoint.y, safe_points[i].x,
safe_points[i].y)
if minDist == 0:
minDist = dist
elif dist < minDist:
minDist = dist
closestToBall_index = i
for i in range(len(safe_points)):
dist2 = calc_pix_dist(robot.centrumX, robot.centrumY,
safe_points[i].x, safe_points[i].y)
index_and_dist = (dist2, i)
safe_point_dist_to_robot.append(index_and_dist)
safe_point_dist_to_robot.sort()
for i in range(2):
if i == 0:
safe_point_index1 = safe_point_dist_to_robot[i][1]
dist3 = calc_pix_dist(safe_points[safe_point_index1].x,
safe_points[safe_point_index1].y,
safe_points[closestToBall_index].x,
safe_points[closestToBall_index].y)
if i == 1:
safe_point_index = safe_point_dist_to_robot[i][1]
dist4 = calc_pix_dist(safe_points[safe_point_index].x,
safe_points[safe_point_index].y,
safe_points[closestToBall_index].x,
safe_points[closestToBall_index].y)
if dist3 < dist4:
first_waypoint = point.Point(
safe_points[safe_point_index1].x,
safe_points[safe_point_index1].y)
else:
first_waypoint = point.Point(
safe_points[safe_point_index].x,
safe_points[safe_point_index].y)
waypoint_list.append(first_waypoint)
direct_path_line = LineString([(first_waypoint.x,
first_waypoint.y),
(endPoint.x, endPoint.y)])
if cm10_pix > center_to_ball_dist:
if lines.areLineTouchingObstacleSquare(direct_path_line):
waypoint_list.append(safe_points[closestToBall_index])
waypoint_list.append(projected_point)
waypoint_list.append(endPoint)
singleton.Singleton.is_in_obstacle = True
singleton.Singleton.is_dangerous = False
# Going for goal
elif singleton.Singleton.is_going_for_goal:
print("Is going for goal")
avoidance = round(20 * track.pixelConversion)
# waypoint_list.append(point.Point(endPoint.x - avoidance, endPoint.y))
if robot.centrumY <= obstacle.center_y:
last_waypoint = point.Point(endPoint.x, endPoint.y - avoidance)
elif robot.centrumY >= obstacle.center_y:
last_waypoint = point.Point(endPoint.x, endPoint.y + avoidance)
if last_waypoint is not None:
avoid_obstacle(last_waypoint)
else:
avoid_obstacle(endPoint)
waypoint_list.append(last_waypoint)
waypoint_list.append(point.Point(endPoint.x, endPoint.y))
singleton.Singleton.is_dangerous = False
singleton.Singleton.is_in_obstacle = False
singleton.Singleton.is_going_for_goal = False
return
# If ball is in top-left corner
elif endPoint.x < track.topLeftCorner.x + danger and endPoint.y < track.topLeftCorner.y + danger:
last_waypoint = point.Point(round(endPoint.x + cornerSafePointX),
round(endPoint.y + cornerSafePointY))
print("Ball is in top left corner")
avoid_obstacle(last_waypoint)
waypoint_list.append(last_waypoint)
waypoint_list.append(point.Point(endPoint.x, endPoint.y))
singleton.Singleton.is_dangerous = True
singleton.Singleton.is_in_obstacle = False
singleton.Singleton.wallOnLeftCorner = True
singleton.Singleton.wallOnRightCorner = False
return
# If ball is in bottom-left corner
elif endPoint.x < track.bottomLeftCorner.x + danger and endPoint.y > track.bottomLeftCorner.y - danger:
last_waypoint = point.Point(round(endPoint.x + cornerSafePointX),
round(endPoint.y - cornerSafePointY))
print("Ball is in bottom left corner")
avoid_obstacle(last_waypoint)
waypoint_list.append(last_waypoint)
waypoint_list.append(point.Point(endPoint.x, endPoint.y))
singleton.Singleton.is_dangerous = True
singleton.Singleton.is_in_obstacle = False
singleton.Singleton.wallOnRightCorner = True
singleton.Singleton.wallOnLeftCorner = False
return
# If ball is in top-right corner
elif endPoint.x > track.topRightCorner.x - danger and endPoint.y < track.topRightCorner.y + danger:
last_waypoint = point.Point(round(endPoint.x - cornerSafePointX),
round(endPoint.y + cornerSafePointY))
print("Ball is in top right corner")
avoid_obstacle(last_waypoint)
waypoint_list.append(last_waypoint)
waypoint_list.append(point.Point(endPoint.x, endPoint.y))
singleton.Singleton.is_dangerous = True
singleton.Singleton.is_in_obstacle = False
singleton.Singleton.wallOnRightCorner = True
singleton.Singleton.wallOnLeftCorner = False
return
# If ball is in bottom-right corner
elif endPoint.x > track.bottomRightCorner.x - danger and endPoint.y > track.bottomRightCorner.y - danger:
last_waypoint = point.Point(round(endPoint.x - cornerSafePointX),
round(endPoint.y - cornerSafePointY))
print("Ball is in bottom right corner")
avoid_obstacle(last_waypoint)
waypoint_list.append(last_waypoint)
waypoint_list.append(point.Point(endPoint.x, endPoint.y))
singleton.Singleton.is_dangerous = True
singleton.Singleton.is_in_obstacle = False
singleton.Singleton.wallOnLeftCorner = True
singleton.Singleton.wallOnRightCorner = False
return
# If ball is close to left side
elif endPoint.x < track.bottomLeftCorner.x + danger:
last_waypoint = point.Point(round(endPoint.x + sideSafePoint),
round(endPoint.y))
print("Ball is close to left side")
avoid_obstacle(last_waypoint)
waypoint_list.append(last_waypoint)
waypoint_list.append(point.Point(endPoint.x, endPoint.y))
singleton.Singleton.is_dangerous = True
singleton.Singleton.is_in_obstacle = False
return
# If ball is close to right side
elif endPoint.x > track.bottomRightCorner.x - danger:
last_waypoint = point.Point(round(endPoint.x - sideSafePoint),
round(endPoint.y))
print("Ball is close to right side")
avoid_obstacle(last_waypoint)
waypoint_list.append(last_waypoint)
waypoint_list.append(point.Point(endPoint.x, endPoint.y))
singleton.Singleton.is_dangerous = True
singleton.Singleton.is_in_obstacle = False
return
# If ball is close to bottom side
elif endPoint.y > track.bottomRightCorner.y - danger:
print("Ball is close to bottom side")
avoid_obstacle(endPoint)
if obstacle.center_x - obstacleDanger < endPoint.x < obstacle.center_x + obstacleDanger:
if robot.centrumX < endPoint.x:
last_waypoint = point.Point(
round(obstacle.center_x - track.pixelConversion * 12),
round(endPoint.y - sideSafePoint))
elif robot.centrumX > endPoint.x:
last_waypoint = point.Point(
round(obstacle.center_x + track.pixelConversion * 12),
round(endPoint.y - sideSafePoint))
else:
last_waypoint = point.Point(round(endPoint.x),
round(endPoint.y - sideSafePoint))
waypoint_list.append(last_waypoint)
waypoint_list.append(point.Point(endPoint.x, endPoint.y))
singleton.Singleton.is_dangerous = True
singleton.Singleton.is_in_obstacle = False
return
# If ball is close to top side
elif endPoint.y < track.topLeftCorner.y + danger:
print("Ball is close to top side")
avoid_obstacle(endPoint)
if obstacle.center_x - obstacleDanger < endPoint.x < obstacle.center_x + obstacleDanger:
if robot.centrumX < endPoint.x:
last_waypoint = point.Point(
round(obstacle.center_x - track.pixelConversion * 12),
round(endPoint.y + sideSafePoint))
elif robot.centrumX > endPoint.x:
last_waypoint = point.Point(
round(obstacle.center_x + track.pixelConversion * 12),
round(endPoint.y + sideSafePoint))
else:
last_waypoint = point.Point(round(endPoint.x),
round(endPoint.y + sideSafePoint))
waypoint_list.append(last_waypoint)
waypoint_list.append(point.Point(endPoint.x, endPoint.y))
singleton.Singleton.is_dangerous = True
singleton.Singleton.is_in_obstacle = False
return
else:
print("Ball is an easy ball")
avoid_obstacle(endPoint)
waypoint_list.append(point.Point(endPoint.x, endPoint.y))
singleton.Singleton.is_dangerous = False
singleton.Singleton.is_in_obstacle = False
print("Are lines touching: " +
str(lines.areLineTouchingObstacleSquare(direct_path_line)))
return
def getObstacle(img):
tempObstacle = obstacle.Obstacle
tempTrack = singleton.Singleton.track
if tempTrack.bottomRightCorner is not None:
pixelConversion = tempTrack.pixelConversion
scale = int(pixelConversion * 5)
low_x_roi = tempTrack.topLeftCorner.x + scale + round(scale / 2)
up_x_roi = tempTrack.bottomRightCorner.x - scale - round(scale / 2)
low_y_roi = tempTrack.topLeftCorner.y + round(1.5 * scale)
up_y_roi = tempTrack.bottomRightCorner.y - round(1.5 * scale)
roi = img[low_y_roi:up_y_roi, low_x_roi:up_x_roi]
blurred_frame = cv2.GaussianBlur(roi, (5, 5), 0)
img_hsv = cv2.cvtColor(blurred_frame, cv2.COLOR_BGR2HSV)
# lower mask (0-10)
lower_red = np.array([0, 150, 20])
upper_red = np.array([10, 255, 255])
mask0 = cv2.inRange(img_hsv, lower_red, upper_red)
# upper mask (170-180)
lower_red = np.array([175, 150, 20])
upper_red = np.array([180, 255, 255])
mask1 = cv2.inRange(img_hsv, lower_red, upper_red)
# join my masks
mask = mask0 + mask1
areaArray = []
count = 1
contours, _ = cv2.findContours(mask,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE,
offset=(low_x_roi, low_y_roi))
for i, c in enumerate(contours):
area = cv2.contourArea(c)
areaArray.append(area)
# first sort the array by area
sorteddata = sorted(zip(areaArray, contours),
key=lambda x: x[0],
reverse=True)
# find the nth largest contour [n-1][1], in this case 2
largestContour = sorteddata[0][1]
# compute the center of the contour
if largestContour is not None:
M = cv2.moments(largestContour)
cX = int(M["m10"] / M["m00"])
cY = int(M["m01"] / M["m00"])
# Pixel correction
x_val = np.amax(img, axis=0)
y_val = np.amax(img, axis=1)
x_val = round(len(x_val) / 2)
y_val = round(len(y_val) / 2)
camera_center = point.Point(x_val, y_val)
obstacle_cen = point.Point(cX, cY)
obstacle_cen_corrected = obstacle_cen_correction(
camera_center, obstacle_cen)
# Set the centerpoint of the obstacle
tempObstacle.center_x = obstacle_cen_corrected.x
tempObstacle.center_y = obstacle_cen_corrected.y
# tempObstacle.center_x = cX
# tempObstacle.center_y = cY
# variables for 20 and 25 in pixels
cm15_in_pix = round(15 * tempTrack.pixelConversion)
cm20_in_pix = round(20 * tempTrack.pixelConversion)
cm25_in_pix = round(25 * tempTrack.pixelConversion)
# makes the top_left corner of the bounding square
top_left_y = cY - cm20_in_pix
top_left_x = cX - cm20_in_pix
tempObstacle.square_top_left_corner = point.Point(
top_left_x, top_left_y)
# makes the top_right corner of the bounding square
top_right_y = cY - cm20_in_pix
top_right_x = cX + cm20_in_pix
tempObstacle.square_top_right_corner = point.Point(
top_right_x, top_right_y)
# makes the bottom_left corner of the bounding square
bottom_left_y = cY + cm20_in_pix
bottom_left_x = cX - cm20_in_pix
tempObstacle.square_bottom_left_corner = point.Point(
bottom_left_x, bottom_left_y)
# makes the bottom_right corner of the bounding square
bottom_right_y = cY + cm20_in_pix
bottom_right_x = cX + cm20_in_pix
tempObstacle.square_bottom_right_corner = point.Point(
bottom_right_x, bottom_right_y)
# makes the lines of the bounding square
tempObstacle.right_line = LineString([(bottom_right_x,
bottom_right_y),
(top_right_x, top_right_y)])
tempObstacle.top_line = LineString([(top_left_x, top_left_y),
(top_right_x, top_right_y)])
tempObstacle.left_line = LineString([(bottom_left_x,
bottom_left_y),
(top_left_x, top_left_y)])
tempObstacle.bottom_line = LineString([
(bottom_left_x, bottom_left_y),
(bottom_right_x, bottom_right_y)
])
# Make midpoints for the dangerzone lines around the obstacle
rightMidPt = tempObstacle.right_line.interpolate(0.5,
normalized=True)
topMidPt = tempObstacle.top_line.interpolate(0.5, normalized=True)
bottomtMidPt = tempObstacle.bottom_line.interpolate(
0.5, normalized=True)
leftMidPt = tempObstacle.left_line.interpolate(0.5,
normalized=True)
# Calculate points on border that are projected 90 degrees from midpoints - 20 cm
a = (int(tempTrack.topRightCorner.x - cm15_in_pix),
int(rightMidPt.y))
b = (int(topMidPt.x),
int(tempTrack.topRightCorner.y + cm15_in_pix))
c = (int(bottomtMidPt.x),
int(tempTrack.bottomRightCorner.y - cm15_in_pix))
d = (int(tempTrack.topLeftCorner.x + cm15_in_pix),
int(leftMidPt.y))
# Find the safepoints that are halfway between the midpoints and the 90 degree points - 20 cm
safePoint1 = LineString([rightMidPt,
a]).interpolate(0.5, normalized=True)
safePoint2 = LineString([topMidPt, b]).interpolate(0.5,
normalized=True)
safePoint3 = LineString([bottomtMidPt,
c]).interpolate(0.5, normalized=True)
safePoint4 = LineString([leftMidPt,
d]).interpolate(0.5, normalized=True)
# Clear old safepoints
singleton.Singleton.safe_points.clear()
# Add to singleton
singleton.Singleton.safe_points.append(
point.Point(safePoint1.x, safePoint1.y))
singleton.Singleton.safe_points.append(
point.Point(safePoint2.x, safePoint2.y))
singleton.Singleton.safe_points.append(
point.Point(safePoint3.x, safePoint3.y))
singleton.Singleton.safe_points.append(
point.Point(safePoint4.x, safePoint4.y))
# draw the contour and center of the shape on the image
cv2.drawContours(img, [largestContour], -1, (0, 255, 0), 2)
cv2.circle(img, (cX, cY), 7, (255, 255, 255), -1)
cv2.putText(img, "center", (cX - 20, cY - 20),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2)
# cv2.imshow("image", img)
# cv2.imshow("mask", mask)
return tempObstacle