def contains_point(self, point):
coords = self.coordinates #polygon['coordinates'][0]
lines = []
#create positive, zero slope ray from point, check for odd
#number of intersects with polygon (ray casting algorithm)
#xy2 = (point[0]+999, point[1])
ray = Line(m=0, b=point[1], x_min=point[0]) #horiz ray to right of point
#print ray
#print line segs
for i in range(0, len(coords)-1):
linexy = [coords[i], coords[i+1]]
#print linexy
line = LineSegment(linexy)
if ray.intersects_line(line):
lines.append(line)
if len(lines) % 2 != 0:
#print '{} intersection(s)'.format(len(lines))
return True
else:
return False
def pipeline(img):
img = cv2.resize(
img, (WIDTH, HEIGHT)
) #resize all videos to one size, to avoid problem of changing code for different size videos
img = cam.undistort_image(img, mtx,
dist) #remove distortion using camera matrix
warped, inverse_perspective = warp_perspective(
img, src, dst
) #warp image to get perspective from src to dist points (found in config.py)
output = segment_lanes(
warped
) #segment lane lines into the image - perform color and sobel thresholds to segment lane lines
left_line = Line() #left lane line object
right_line = Line() #right lane line object
left_line, right_line, window_search_img = window_search(
output, left_line, right_line, 4
) #perform window search to get left and right lanes and fit their positions
line_image = draw_lines_on_undistorted(
img, inverse_perspective, left_line,
right_line) #draw lines on unwarped image
mean_curvature_meter = np.mean(
[left_line.curvature_meter,
right_line.curvature_meter]) #calculate radius of curvature
cv2.putText(line_image,
'Curvature radius: {:.02f}m'.format(mean_curvature_meter),
(100, 60), cv2.FONT_HERSHEY_DUPLEX, 1, (255, 255, 255), 2,
cv2.LINE_AA)
center_offset = offset_from_center(
line_image, left_line, right_line) #computer offset from center
cv2.putText(line_image,
'Offset from Center: {:.02f}m'.format(center_offset),
(100, 120), cv2.FONT_HERSHEY_PLAIN, 2, (255, 255, 255), 2,
cv2.LINE_AA)
cv2.imshow("Window search", window_search_img)
return line_image
def go_to_coord(self, new):
line_start = None
while (self.x, self.y) != new:
#point toward the target
yield from self.turn_and_back_away(
math.atan((new[1] - self.y) / (new[0] - self.x)))
#go to target (if possible)
distance = math.sqrt((self.y - new[1])**2 + (self.x - new[0])**2)
curr = time.time()
l, r, ok = yield self.move_forward(distance)
self.record_motion(curr, time.time(), distance, ok)
#didn't work?!
if not ok:
#record where we're at and turn the smallest reasonable amount
line_start = self.location()
angle = math.atan((l - r) / SENSOR_SEPARATION)
yield from self.turn_and_back_away(angle)
# recall whether the obstacle is on the left or right
o_left = l > r
#move a bit forward
curr = time.time()
l, r, ok = yield self.move_forward(0.1)
self.record_motion(curr, time.time(), 0.1, ok)
#we can't move forward - we have a corner case
if not ok:
#turn away from the known wall, along the new one
angle = math.atan((l - r) / SENSOR_SEPARATION)
if o_left == l < r:
angle -= math.pi
yield from self.turn_and_back_away(angle)
#move alogn the second wall for a bit.
curr = time.time()
l, r, ok = yield self.move_forward(0.1)
self.record_motion(curr, time.time(), 0.1, ok)
#if you're at a third corner, give up
if not ok:
raise Exception("Andrea, why'd you put it in a box?")
elif line_start is not None:
Line(line_start, self.location()).clean_firebase()
def __init__( self, startPoint, endPoint, headSize ):
Line.__init__( self, startPoint, endPoint )
self.headSize = headSize
def soccer():
global pub_command, pub_stop, TARGET_OFFSET, file
rospy.init_node("soccer_node")
rospy.Subscriber("/emergency_stop", Empty, closer)
location.init()
rospy.on_shutdown(cleanUp)
file = open("soccer.log", "w")
print "Connecting..."
while pub_command.get_num_connections() == 0:
pass
x = 0
y = 0
theta = 0
true_angle = 0
file.write("{} {}\n".format(x, y))
print["start", x, y]
# scan from current position for ball and goal
ball_angle_1, goal_angle_1 = scan(pub_command)
rospy.sleep(1)
# turn to face 45 degrees past ball away from goal
if ball_angle_1 < 0:
ball_angle_1 += 360
if goal_angle_1 < 0:
goal_angle_1 += 360
direction = 1
delta = goal_angle_1 - ball_angle_1
if delta < -180 or (delta > 0 and delta < 180):
direction = -1
target_angle = ball_angle_1 + direction * 45
print "--- SCAN 1 RESULTS ---"
print "x: {} y: {}, goal @ {}, ball @ {}".format(x, y, goal_angle_1,
ball_angle_1)
goal_vector_1 = Line(x=x, y=y, theta=goal_angle_1, useDegrees=True)
ball_vector_1 = Line(x=x, y=y, theta=ball_angle_1, useDegrees=True)
_, _, theta = location.currentLocation
if theta < 0:
theta += 360
move_angle = target_angle - theta
execute(0, move_angle, 0.6)
rospy.sleep(1)
# move to new location
move_dist = 0.3
execute(move_dist, 0, 0.5)
print "--- REALIGNING FOR SCAN 2 ---"
print "odom @ {}, angle from scan 1 start @ {}".format(
location.currentLocation[2], target_angle)
true_angle = target_angle
scan_2_vector = Line(x=0, y=0, theta=true_angle, useDegrees=True)
x, y = scan_2_vector.findPointFrom(0, 0, move_dist)
file.write("{} {}\n".format(x, y))
print["scan_2", x, y]
# get new position
_, _, theta = location.currentLocation
theta += true_angle
# scan from current position for ball and goal
ball_angle_2, goal_angle_2 = scan(pub_command)
ball_angle_2 += true_angle
goal_angle_2 += true_angle
goal_vector_2 = Line(x=x, y=y, theta=goal_angle_2, useDegrees=True)
ball_vector_2 = Line(x=x, y=y, theta=ball_angle_2, useDegrees=True)
# calculate intersections
goal_x, goal_y = goal_vector_1.intersect(goal_vector_2)
ball_x, ball_y = ball_vector_1.intersect(ball_vector_2)
print "--- SCAN 2 RESULTS ---"
print "x: {}, y: {}, goal @ {}, ball @ {}".format(x, y, goal_angle_2,
ball_angle_2)
print "--- GOAL LOCATION ---"
print "x: {}, y: {}".format(goal_x, goal_y)
print "--- BALL LOCATION ---"
print "x: {}, y: {}".format(ball_x, ball_y)
file.write("{} {}\n".format(ball_x, ball_y))
file.write("{} {}\n".format(goal_x, goal_y))
print["ball", ball_x, ball_y]
print["goal", goal_x, goal_y]
# find line from ball to goal
approach_vector = Line(x1=goal_x, y1=goal_y, x2=ball_x, y2=ball_y)
TARGET_OFFSET = 0.5
# select point on approach vector for robot to start
target_x, target_y = approach_vector.findPointFrom(ball_x, ball_y,
TARGET_OFFSET)
alt_x = ball_x * 2 - target_x
alt_y = ball_y * 2 - target_y
file.write("{} {}\n".format(target_x, target_y))
print["kick", target_x, target_y]
file.write("{} {}\n".format(alt_x, alt_y))
print["kick_alt", alt_x, alt_y]
d_ball_target = distance(ball_x, ball_y, target_x, target_y)
d_ball_goal = distance(ball_x, ball_y, goal_x, goal_y)
d_goal_target = distance(goal_x, goal_y, target_x, target_y)
d_goal_alt = distance(goal_x, goal_y, alt_x, alt_y)
if d_goal_alt > d_goal_target:
print "--- ALT SELECTED ---"
target_x = alt_x
target_y = alt_y
else:
print "--- FIRST TARGET SELECTED ---"
print "x: {}, y: {}".format(target_x, target_y)
# find way to move from current position to target pt
dist = distance(x, y, target_x, target_y)
movement_vector = Line(x=x, y=y, x2=target_x, y2=target_y)
angle = movement_vector.angle(useDegrees=True)
if angle < 0:
angle += 360
# check if the angle is correct or opposite of correct
if ball_angle_2 > goal_angle_2:
print "--- USING ALTERNATE ANGLE ---"
if angle < ball_angle_2:
angle += 180
if angle > ball_angle_2 + 180:
angle -= 180
print "--- APPROACHING SHOT LOCATION ---"
print "angle to shot location @ {}".format(angle)
_, _, theta = location.currentLocation
theta += true_angle
execute(0, angle - theta, 0.6, reset=True)
execute(dist, 0, 0.6, reset=True)
print "--- REALIGNING FOR SHOT ---"
print "odom @ {}, angle from scan 2 start @ {}, angle from scan 1 start @ {}".format(
location.currentLocation[2], angle, angle + true_angle)
# scan for ball(and goal)
ball_angle_fin, goal_angle_fin = scan(pub_command)
# create background thread to stop robot once ball is hit
from threading import Thread
stop_thread = Thread(target=stopper)
stop_thread.start()
# make the shot
execute(TARGET_OFFSET + 2, 0, 1, reset=True)
stop_thread.join()
file.close()
file = None
def __init__(self, startPoint, endPoint, headSize):
Line.__init__(self, startPoint, endPoint)
self.headSize = headSize
screen = pg.display.set_mode(settings.win_size,
flags=pg.DOUBLEBUF | pg.NOFRAME)
surf = pg.Surface(settings.win_size)
surf.set_alpha(200)
settings.screen = screen
settings.main_surf = surf
clock = pg.time.Clock()
font = pg.font.SysFont('Times New Roman', 16)
lines = []
for k in range(settings.lines_num):
line = Line(settings)
line.draw()
lines.append(line)
while True:
surf.fill(settings.win_bg)
clock.tick(settings.win_fps)
mouse_rad_rect = pg.draw.circle(settings.main_surf, (255, 0, 0),
settings.mouse_pos, settings.mouse_rad, 1)
for line_ind in settings.mouse_rect.collidelistall([k.rect
for k in lines]):
line = lines[line_ind]
if settings.mouse_pos[1] <= line.y:
# Read in the saved camera matrix and distortion coefficients
# These are the arrays you calculated using cv2.calibrateCamera()
dist_pickle = pickle.load(open("configs/camera_params.p", "rb"))
mtx = dist_pickle["mtx"]
dist = dist_pickle["dist"]
# project Videos
project_video = 'project_video.mp4'
# project_video = 'challenge_video.mp4'
# video output path
video_output_path = 'output_images/project_video.mp4'
# Declare lines
right_line = Line()
left_line = Line()
# create a video capture
video_capture = cv2.VideoCapture(project_video)
# define a video writer
video_writer = cv2.VideoWriter(video_output_path,
fourcc=cv2.VideoWriter_fourcc(*'MP4V'),
fps=30,
frameSize=(1280, 720))
## Parameter settings
# Thresholding params
color_thresh = (170, 255)
sobel_thresh = (50, 100)
text1 = 'Radius of Curvature: %d(m)'
text2 = 'Offset from center: %.2f(m)'
text3 = 'Radius of Curvature: Inf (m)'
if mean_curv < 3000:
cv2.putText(result, text1 % (int(mean_curv)),
(60, 100), font, 1.0, (255, 255, 255), thickness=2)
else:
cv2.putText(result, text3,
(60, 100), font, 1.0, (255, 255, 255), thickness=2)
cv2.putText(result, text2 % (-offset),
(60, 130), font, 1.0, (255, 255, 255), thickness=2)
return result
c = Camera(display=False)
# Video Pipline
frame_num = 15 # latest number of frames for good detection
left = Line(False, frame_num)
right = Line(True, frame_num)
video_output = 'result.mp4'
input_path = './test_videos/project_video.mp4'
clip1 = VideoFileClip(input_path).subclip(0, 30)
final_clip = clip1.fl_image(lambda image: process_image(image, c, left, right))
final_clip.write_videofile(video_output, audio=False)
f = open('data/distortion_correction.pckl', 'r')
mtx = pickle.load(f)
dist = pickle.load(f)
M = pickle.load(f)
Minv = pickle.load(f)
else:
mtx, dist = calibrate_camera()
M, Minv = compute_perspective_distortion()
f = open('data/distortion_correction.pckl', 'wb')
pickle.dump(mtx, f)
pickle.dump(dist, f)
pickle.dump(M, f)
pickle.dump(Minv, f)
# Line objects to keep track of the detected data
leftLine = Line(niterations=3, height=720, width=1280, M=M)
rightLine = Line(niterations=3, height=720, width=1280, M=M)
cap = cv2.VideoCapture(video_file)
# Variables for inter-prediction filter. Initialised to None since for the very
# first frame there is no temporal information available.
inter_reference = None
plotyy = None
cnt = 0
while (cap.isOpened()):
ret, img = cap.read()
if img is None:
break
def side_right(self):
return Line(vec(self.right, self.top), vec(self.right, self.bottom))
def side_left(self):
return Line(vec(self.left, self.top), vec(self.left, self.bottom))
def side_bottom(self):
return Line(vec(self.left, self.bottom), vec(self.right, self.bottom))
def side_top(self):
return Line(vec(self.left, self.top), vec(self.right, self.top))
