def __init__(self,
speed=0.15,
lane_offset=140,
wait_period=10,
hard_coded_turns=True):
self.hard_coded_turns = hard_coded_turns
self.speed = speed
self.pid = SteeringPid(lane_offset, kp=0.1, ki=0.006, kd=1.2)
self.intersections_pid = SteeringPid(1, kp=0.1, ki=0.006, kd=1.2)
self.current_turn = None
self.current_turn_direction = None
self.handling_intersection = False
self.inter_start_time = time.time()
self.go_straight = False
self.angle = 0
self.waypoint = Waypoint()
self.car = Car_Control()
self.detector = Image_Process()
self.vs = cv2.VideoCapture(
"/dev/video2",
cv2.CAP_V4L) # TODO: figure out a good way to do this
self.intersections = Intersections(wait_period=wait_period)
self.pipeline = rs.pipeline()
self.config = rs.config()
self.config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
self.pipeline.start(self.config)
def local_intersect(self, ray):
if abs(ray.direction.y) < EPSILON:
return Intersections([])
else:
t1 = -ray.origin.y / ray.direction.y
i1 = Intersection(t1, self)
return Intersections([i1])
def test_hit3(self):
s = Sphere()
i1 = Intersection(-2, s)
i2 = Intersection(-1, s)
xs = Intersections([i1, i2])
i = xs.hit()
self.assertTrue(i == None)
def test_hit1(self):
s = Sphere()
i1 = Intersection(1, s)
i2 = Intersection(2, s)
xs = Intersections([i1, i2])
i = xs.hit()
self.assertTrue(i == i1)
def test_hit4(self):
s = Sphere()
i1 = Intersection(5, s)
i2 = Intersection(7, s)
i3 = Intersection(-3, s)
i4 = Intersection(2, s)
xs = Intersections([i1, i2, i3, i4])
i = xs.hit()
self.assertTrue(i == i4)
def local_intersect(self, ray):
xtmin, xtmax = self.check_axis(ray.origin.x, ray.direction.x)
ytmin, ytmax = self.check_axis(ray.origin.y, ray.direction.y)
ztmin, ztmax = self.check_axis(ray.origin.z, ray.direction.z)
tmin = max(xtmin, ytmin, ztmin)
tmax = min(xtmax, ytmax, ztmax)
if tmin > tmax:
return Intersections([])
ls = [Intersection(tmin, self), Intersection(tmax, self)]
xs = Intersections(ls)
return xs
def local_intersect(self, ray):
# ray2 = self.transform.inverse() * ray
# ray = Ray(self.transform.inverse()*ray.origin, self.transform.inverse()*ray.direction)
sphere2ray = ray.origin - Point(0, 0, 0)
a = ray.direction.dot(ray.direction)
b = 2 * ray.direction.dot(sphere2ray)
c = sphere2ray.dot(sphere2ray) - 1
discriminant = b * b - 4 * a * c
if discriminant < 0:
return Intersections([])
else:
t1 = (-b - sqrt(discriminant)) / (2 * a)
t2 = (-b + sqrt(discriminant)) / (2 * a)
i1 = Intersection(t1, self)
i2 = Intersection(t2, self)
return Intersections([i1, i2])
def test_intersect3(self):
origin = Point(0, 2, -5)
direction = Vector(0, 0, 1)
r = Ray(origin, direction)
s = Sphere()
xs = Intersections([])
self.assertTrue(s.intersect(r) == xs)
def test_int2(self):
s = Sphere()
i1 = Intersection(1, s)
i2 = Intersection(2, s)
xs = Intersections([i1, i2])
self.assertEqual(xs.count, 2)
self.assertEqual(xs[0].t, 1)
self.assertEqual(xs[1].t, 2)
def test_intersect5(self):
origin = Point(0, 0, 5)
direction = Vector(0, 0, 1)
r = Ray(origin, direction)
s = Sphere()
i1 = Intersection(-6, s)
i2 = Intersection(-4, s)
xs = Intersections([i1, i2])
self.assertTrue(s.intersect(r) == xs)
def test_schlick_approximation_with_small_angle_and_n2_larger_than_n1(self):
shape = GlassSphere()
r = Ray(Point(0, 0.99, -2), Vector(0, 0, 1))
ls = [
Intersection(1.8589, shape),
]
xs = Intersections(ls)
comps = xs[0].prepare_computations(r, xs)
reflectance = comps.schlick()
self.assertTrue(equals(reflectance, 0.48873))
def test_schlick_approximation_with_perpendicular_viewing_angle(self):
shape = GlassSphere()
r = Ray(Point(0, 0, 0), Vector(0, 1, 0))
ls = [
Intersection(-1, shape),
Intersection(1, shape)
]
xs = Intersections(ls)
comps = xs[1].prepare_computations(r, xs)
reflectance = comps.schlick()
self.assertTrue(equals(reflectance, 0.04))
def test_schlick_approximation_under_total_internal_reflection(self):
shape = GlassSphere()
r = Ray(Point(0, 0, sqrt(2)/2), Vector(0, 1, 0))
ls = [
Intersection(-sqrt(2)/2, shape),
Intersection(sqrt(2)/2, shape)
]
xs = Intersections(ls)
comps = xs[1].prepare_computations(r, xs)
reflectance = comps.schlick()
self.assertTrue(equals(reflectance, 1.0))
def test_under_point1(self):
r = Ray(Point(0, 0, -5), Vector(0, 0, 1))
shape = GlassSphere()
shape.set_transform(translate(0, 0, 1))
i = Intersection(5, shape)
xs = Intersections([i])
comps = i.prepare_computations(r, xs)
# print(comps.normalv)
# print(comps.point)
# print(comps.under_point.z)
self.assertTrue(comps.under_point.z > EPSILON / 2)
self.assertTrue(comps.point.z < comps.under_point.z)
def test_refracted_color_opaque_surface(self):
w = World()
shape = w.objs[0]
r = Ray(Point(0, 0, -5), Vector(0, 0, -1))
ls = [
Intersection(4, shape),
Intersection(6, shape)
]
xs = Intersections(ls)
comps = xs[0].prepare_computations(r, xs)
c = w.refracted_color(comps, 5)
self.assertTrue(Color(0, 0, 0) == c)
def test_refracted_color_at_maximum_recursive_depth(self):
w = World()
shape = w.objs[0]
shape.material.transparency = 1.0
shape.material.refractive_index = 1.5
r = Ray(Point(0, 0, -5), Vector(0, 0, -1))
ls = [
Intersection(4, shape),
Intersection(6, shape)
]
xs = Intersections(ls)
comps = xs[0].prepare_computations(r, xs)
c = w.refracted_color(comps, 0)
self.assertTrue(Color(0, 0, 0) == c)
def test_refract1(self):
A = GlassSphere()
A.set_transform(scale(2, 2, 2))
A.material.refractive_index = 1.5
B = GlassSphere()
B.set_transform(translate(0, 0, -0.25))
B.material.refractive_index = 2.0
C = GlassSphere()
C.set_transform(translate(0, 0, 0.25))
C.material.refractive_index = 2.5
r = Ray(Point(0, 0, -4), Vector(0, 0, 1))
ls = [
Intersection(2, A),
Intersection(2.75, B),
Intersection(3.25, C),
Intersection(4.75, B),
Intersection(5.25, C),
Intersection(6, A)
]
xs = Intersections(ls)
comps0 = xs[0].prepare_computations(r, xs)
self.assertEqual(comps0.n1, 1.0)
self.assertEqual(comps0.n2, 1.5)
comps1 = xs[1].prepare_computations(r, xs)
self.assertEqual(comps1.n1, 1.5)
self.assertEqual(comps1.n2, 2.0)
comps2 = xs[2].prepare_computations(r, xs)
self.assertEqual(comps2.n1, 2.0)
self.assertEqual(comps2.n2, 2.5)
comps3 = xs[3].prepare_computations(r, xs)
self.assertEqual(comps3.n1, 2.5)
self.assertEqual(comps3.n2, 2.5)
comps4 = xs[4].prepare_computations(r, xs)
self.assertEqual(comps4.n1, 2.5)
self.assertEqual(comps4.n2, 1.5)
comps5 = xs[5].prepare_computations(r, xs)
self.assertEqual(comps5.n1, 1.5)
self.assertEqual(comps5.n2, 1.0)
def test_refracted_color_under_total_internal_reflection(self):
w = World()
shape = w.objs[0]
shape.material.transparency = 1.0
shape.material.refractive_index = 1.5
r = Ray(Point(0, 0, sqrt(2)/2), Vector(0, 1, 0))
ls = [
Intersection(-sqrt(2)/2, shape),
Intersection(sqrt(2)/2, shape)
]
xs = Intersections(ls)
comps = xs[1].prepare_computations(r, xs)
c = w.refracted_color(comps, 5)
self.assertTrue(c == Color(0, 0, 0))
def intersect(self, ray):
def swap(l, i1, i2):
temp = l[i1]
l[i1] = l[i2]
l[i2] = temp
return l
ls = []
for obj in self.objs:
intersections = obj.intersect(ray)
if intersections.count != 0:
for i in range(intersections.count):
ls.append(intersections[i])
n = len(ls)
for i in range(1, n):
target = ls[i].t
for j in range(i):
if ls[j].t > ls[i].t:
l = swap(ls, i, j)
return Intersections(ls)
def test_shade_hit_with_reflective_transparent_material(self):
w = World()
r = Ray(Point(0, 0, -3), Vector(0, -sqrt(2)/2, sqrt(2)/2))
floor = Plane()
floor.set_transform(translate(0, -1, 0))
floor.material.reflective = 0.5
floor.material.transparency = 0.5
floor.material.refractive_index = 1.5
w.objs.append(floor)
ball = Sphere()
ball.material.color = Color(1, 0, 0)
ball.material.ambient = 0.5
ball.set_transform(translate(0, -3.5, -0.5))
w.objs.append(ball)
ls = [Intersection(sqrt(2), floor)]
xs = Intersections(ls)
comps = xs[0].prepare_computations(r, xs)
color = w.shade_hit(comps, 5)
# print(color)
self.assertTrue(Color(0.93391, 0.69643, 0.69243) == color)
def test_shade_hit_with_transparent_material(self):
w = World()
floor = Plane()
floor.set_transform(translate(0, -1, 0))
floor.material.transparency = 0.5
floor.material.refractive_index = 1.5
ball = Sphere()
ball.material.color = Color(1., 0., 0.)
ball.material.ambient = 0.5
ball.set_transform(translate(0, -3.5, -0.5))
w.objs = [floor, ball]
r = Ray(Point(0, 0, -3), Vector(0, -sqrt(2)/2, sqrt(2)/2))
ls = [Intersection(sqrt(2), floor)]
xs = Intersections(ls)
comps = xs[0].prepare_computations(r, xs)
color = w.shade_hit(comps, 5)
self.assertTrue(Color(0.93642, 0.68642, 0.68642) == color)
def test_refracted_color_with_refracted_ray(self):
# this test failed!
w = World()
A = w.objs[0]
A.material.ambient = 1
A.material.pattern = TestPattern()
B = w.objs[1]
B.material.transparency = 1.0
B.material.refractive_index = 1.5
r = Ray(Point(0, 0, 0.1), Vector(0, 1, 0))
ls = [
Intersection(-0.9899, A),
Intersection(-0.4899, B),
Intersection(0.4899, B),
Intersection(0.9899, A)
]
xs = Intersections(ls)
comps = xs[2].prepare_computations(r, xs)
c = w.refracted_color(comps, 5)
# the expected answer is color(0, 0.99888, 0.04725)
self.assertTrue(c == Color(0, 0.99888, 0.04721))
def scan(main_color, intermediates=None):
main_gray = utility.shrink_img(main_color) # 処理速度を上げるため縮小する
main_gray = cv2.cvtColor(main_gray, cv2.COLOR_BGR2GRAY)
size_color = main_color.shape[:2]
size_gray = main_gray.shape
length_threshold = 20
distance_threshold = 1.4142
# canny_th1 = 5.0
# canny_th2 = 50.0
canny_th1 = 1.0
canny_th2 = 10
canny_size = 3
do_merge = False
do_merge = False
fld = cv2.ximgproc.createFastLineDetector(length_threshold,
distance_threshold, canny_th1,
canny_th2, canny_size, do_merge)
line_pnts = fld.detect(main_gray)
line_pnts = np.array(line_pnts).reshape((-1, 4))
lines = Lines(line_pnts)
print(f"Num of lines: {lines.num} => ", end="")
lines.remove_central(size_gray)
print(lines.num)
equal = lines.equal()
labels = partition.partition(lines.num, equal)
labels_num = len(np.unique(labels))
if intermediates is not None:
intermediates['lines'] = utility.draw_lines(main_gray, lines, labels,
labels_num)
cv2.imwrite("lines.jpeg", intermediates['lines'])
segments = Segments(lines, labels, labels_num, size_gray)
print(f"Num of segments: {segments.num} => ", end="")
segments.remove_central(size_gray)
print(segments.num)
if intermediates is not None:
intermediates['segments'] = utility.draw_segments(main_gray, segments)
cv2.imwrite("segments.jpeg", intermediates['segments'])
intersections = Intersections(segments, size_gray)
print(f"Num of intersections: {intersections.num}")
if intermediates is not None:
intermediates['intersections'] = utility.draw_intersections(
main_gray, intersections)
cv2.imwrite("intersections.jpeg", intermediates['intersections'])
df = ml_model.prepare_data(intersections, size_gray)
scores = ml_model.get_score(df)
indice = np.argsort(scores)[::-1]
points_per_section = 3
vertex_lt = []
vertex_rt = []
vertex_lb = []
vertex_rb = []
for idx in indice:
if intersections.is_left[idx] and intersections.is_top[idx] and len(
vertex_lt) < points_per_section:
vertex_lt.append(idx)
elif intersections.is_right[idx] and intersections.is_top[idx] and len(
vertex_rt) < points_per_section:
vertex_rt.append(idx)
elif intersections.is_left[idx] and intersections.is_bottom[
idx] and len(vertex_lb) < points_per_section:
vertex_lb.append(idx)
elif intersections.is_right[idx] and intersections.is_bottom[
idx] and len(vertex_rb) < points_per_section:
vertex_rb.append(idx)
if len(vertex_lt) >= points_per_section and \
len(vertex_rt) >= points_per_section and \
len(vertex_lb) >= points_per_section and \
len(vertex_rb) >= points_per_section:
break
if len(vertex_lt) == 0:
print("no vertex at left top was found")
return None
if len(vertex_rt) == 0:
print("no vertex at right top was found")
return None
if len(vertex_lb) == 0:
print("no vertex at left bottom was found")
return None
if len(vertex_rb) == 0:
print("no vertex at right bottom was found")
return None
idx_lt, idx_rt, idx_lb, idx_rb = utility.get_best_set(
intersections, scores, vertex_lt, vertex_rt, vertex_lb, vertex_rb)
# print(f"selected vertexes: ({idx_lt}, {idx_rt}, {idx_lb}, {idx_rb})")
if intermediates is not None:
intermediates['detected'] = utility.draw_detected(
main_gray, intersections, idx_lt, idx_rt, idx_lb, idx_rb)
src_points_gray = np.array([ \
intersections.cross_pnt[idx_lt], \
intersections.cross_pnt[idx_rt], \
intersections.cross_pnt[idx_lb], \
intersections.cross_pnt[idx_rb]
], dtype=np.float32)
dst_points_gray = np.array([ \
(0, 0),
(size_gray[1], 0),
(0, size_gray[0]),
(size_gray[1], size_gray[0])
], dtype=np.float32)
scale = np.array(size_color, dtype=np.float32) / np.array(size_gray,
dtype=np.float32)
scale = scale[::-1]
src_points_color = src_points_gray * scale
dst_points_color = dst_points_gray * scale
M_color = cv2.getPerspectiveTransform(src_points_color, dst_points_color)
main_color = cv2.warpPerspective(main_color, M_color, size_color[::-1])
return main_color
class Self_Drive:
def __init__(self,
speed=0.15,
lane_offset=150,
wait_period=10,
hard_coded_turns=True):
self.hard_coded_turns = hard_coded_turns
self.speed = speed
self.pid = SteeringPid(lane_offset, kp=0.1, ki=0.006, kd=1.2)
self.intersections_pid = SteeringPid(1, kp=0.1, ki=0.006, kd=1.2)
self.current_turn = None
self.current_turn_direction = None
self.handling_intersection = False
self.angle = 0
#self.waypoint = Waypoint()
self.car = Car_Control()
self.detector = Image_Process()
self.vs = cv2.VideoCapture(
"/dev/video2",
cv2.CAP_V4L) # TODO: figure out a good way to do this
self.intersections = Intersections(wait_period=wait_period)
self.pipeline = rs.pipeline()
self.config = rs.config()
self.config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
self.pipeline.start(self.config)
def self_drive(self):
global run
global image
global white
global yellow
global frame
global dst
global api_speed
global depth_colormap
global run_yolo_bool
global stop_at_light
self.car.steer(0.0)
if run:
self.car.drive(self.speed)
yolo_run_count = 0
while True:
try:
# Wait for a coherent pair of frames: depth and color
frames = self.pipeline.wait_for_frames()
depth_frame = frames.get_depth_frame()
grabbed, frame = self.vs.read()
speed = self.speed
if not grabbed or not depth_frame:
print("Couldn't Grab Next Frame")
break
yolo_run_count += 1
if yolo_run_count == 30:
run_yolo_bool = True
yolo_run_count = 0
depth_image = np.asanyarray(depth_frame.get_data())
#print(depth_image[220][200:400])
depth_colormap = cv2.applyColorMap(
cv2.convertScaleAbs(depth_image, alpha=0.03),
cv2.COLORMAP_HSV)
offset, image, white, yellow = self.detector.offset_detect(
frame)
self.angle = self.pid.run_pid(offset)
start = 200 #int(200 + 3 * self.angle)
end = 400 #int(400 + 3 * self.angle)
cv2.rectangle(depth_colormap, (start, 210), (end, 230),
(255, 0, 0), 5)
num = 0
for i in range(start, end):
#for j in range(210,230):
if depth_image[220][i] < 600 and depth_image[220][i] > 0:
num += 1
if num >= 20:
obstacle = True
else:
obstacle = False
#print(run, obstacle, stop_at_light, dst, api_speed)
if run and not obstacle and not stop_at_light:
self._handle_intersection()
self.car.drive(api_speed)
self.car.steer(self.angle)
else:
self.car.drive(0)
except Exception as e:
print(repr(e))
self.car.stop()
def _find_closest(self, point, turn, direction):
distance_values = distance.cdist([point], TURNS[turn][direction])
closest_index = distance_values.argmin()
if closest_index != len(distance_values) - 1:
closest_index = closest_index + 1
# print(distance_values[0][closest_index])
return TURNS[turn][direction][closest_index]
def _handle_intersection(self):
global mo
intersection, turn = self.intersections.get_intersection()
# if self.handling_intersection and not self.hard_coded_turns:
# cur_pos, old_pos = GPS.get_gps_all()
# des_pos = self._find_closest(cur_pos, self.current_turn, self.current_turn_direction)
# angle, _ = Translate.get_angle(cur_pos, old_pos, des_pos)
# # should break us out of the intersection handling mode.
# # this checks to see if the point we are closest to is the last point in the intersection
# if des_pos == TURNS[self.current_turn][self.current_turn_direction][-1]:
# self.handling_intersection = False
# self.angle = angle/2
# # self.angle = self.intersections_pid.run_pid(angle)
# print("Cur Pos: {}, Old Pos: {}, Des Pos; {}".format(cur_pos, old_pos, des_pos))
# print("Turn angle: {}".format(self.angle))
if intersection:
self.handling_intersection = True
self.current_turn = turn
if self.current_turn in ["2", "3", "4", "5"]:
self.current_turn_direction = "STRAIGHT"
elif self.current_turn in ["0", "7"]:
self.current_turn_direction = "LEFT"
#self.car.stop()
#time.sleep(1)
elif self.current_turn in ["1", "6"]:
self.current_turn_direction = "RIGHT"
#self.car.stop()
#time.sleep(1)
print("Turning {} at {}".format(self.current_turn_direction,
self.current_turn))
#mo.start_run(10000)
if self.hard_coded_turns:
self.handling_intersection = False
if self.current_turn in ["2", "3", "4", "5"
]: #at the four way, tune seperately
if self.current_turn_direction == "RIGHT":
self.car.right(wait=0.9,
duration=2.5,
angle=30,
speed=(self.speed + 0.3))
elif self.current_turn_direction == "LEFT":
#self.car.left(wait=0+1.8, duration=1.8, angle=-20, speed=(self.speed+ 0.3))
self.car.mouse_left(wait=23000,
duration=23000,
angle=-20,
speed=(self.speed + 0.3))
elif self.current_turn_direction == "STRAIGHT":
#self.car.straight(wait=0+.75,duration=1.75, angle=0, speed=(self.speed + 0.3))
self.car.mouse_straight(wait=8000,
duration=24000,
angle=0,
speed=(self.speed + 0.3))
elif self.current_turn_direction == "RIGHT":
#self.car.right(wait=0.0+.1, duration=2, angle=30, speed=(self.speed + 0.3))
self.car.mouse_right(wait=2000,
duration=12000,
angle=30,
speed=(self.speed + 0.3),
stop=1)
elif self.current_turn_direction == "LEFT":
#self.car.left(wait=0.2+1, duration=2, angle=-20, speed=(self.speed+ 0.3))
self.car.mouse_left(wait=9000,
duration=15000,
angle=-30,
speed=(self.speed + 0.3),
stop=1)
def houghlinemethod(self, edges):
lines = cv2.HoughLines(edges, 0.7, np.pi / 500, 130)
for rho, theta in lines[0]:
a = np.cos(theta)
b = np.sin(theta)
x0 = a * rho
y0 = b * rho
x1 = int(x0 + 1000 * (-b))
y1 = int(y0 + 1000 * (a))
x2 = int(x0 - 1000 * (-b))
y2 = int(y0 - 1000 * (a))
cv2.line(self.source_color, (x1, y1), (x2, y2), (0, 255, 255), 1)
# Initial Intersection class
inter = Intersections()
for i in range(0, lines.shape[1]):
for j in range(i + 1, lines.shape[1]):
line1 = lines[0][i]
line2 = lines[0][j]
# check the lines as a pair to compute their intersections
if inter.acceptLinePair(line1, line2, np.pi / 32):
intersection = inter.computeintersect(line1, line2)
#print intersection
if (intersection[0] > 0 and intersection[1] > 0):
inter.append(intersection[0], intersection[1])
cv2.circle(self.source_color,
(intersection[0], intersection[1]), 6,
(0, 255, 255), 2)
# combination of the extracted intersection with harris corner detetction
gray = cv2.cvtColor(self.source, cv2.COLOR_BGR2GRAY)
'''harris corner detector and draw red circle around them'''
self.features = cv2.goodFeaturesToTrack(gray,
1000,
0.001,
5,
None,
None,
2,
useHarrisDetector=True,
k=0.00009)
if len(self.features.shape) == 3.0:
assert (self.features.shape[1:] == (1, 2))
self.features.shape = (self.features.shape[0], 2)
for x, y in self.features:
self.corners += [(x, y)]
inter.append_features(x, y)
cv2.circle(self.source_color, (x, y), 8, (255, 255, 255))
cv2.imwrite("images/result_images/Lines_intersection.jpg",
self.source_color)
#cv2.imshow("lines and intersections", self.source_color)
#cv2.waitKey(0)
# subpixel accuracy
'''define the criteria to stop and refine the corners'''
criteria = (cv2.TERM_CRITERIA_MAX_ITER | cv2.TERM_CRITERIA_EPS, 100,
0.03)
#criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.001)
cv2.cornerSubPix(gray, self.features, (5, 5), (-1, -1), criteria)
for x, y in self.features:
self.refined_corners += [(x, y)]
inter.append_refined_features(x, y)
cv2.circle(self.source_color, (x, y), 4, (0, 255, 0))
'''Now draw them'''
res = np.hstack((self.corners, self.refined_corners))
res = np.int0(res)
self.source_color[res[:, 1], res[:, 0]] = [0, 0, 255]
self.source_color[res[:, 3], res[:, 2]] = [0, 255, 0]
#cv2.imwrite(str(self.IMAGE_NAME) + '_subpixel.png', self.im_new)
#cv2.imshow("corners", self.source_color)
#cv2.waitKey(0)
return inter
def houghlinePmethod(self, edges):
minLineLength = int(self.source_color.shape[0] / 5.0)
maxLineGap = int(self.source_color.shape[0] / 2.0)
linesP = cv2.HoughLinesP(edges, 1, np.pi / 180, 95, np.array([]),
minLineLength, maxLineGap)
print "linesP are: " + str(linesP)
for x1, y1, x2, y2 in linesP[0]:
#cv2.circle(self.source_color, (x1, y1), 6, (0, 255, 255), 1)
#cv2.circle(self.source_color, (x2, y2), 6, (255, 0, 255), 1)
cv2.line(self.source_color, (x1, y1), (x2, y2), (0, 0, 255))
#cv2.imshow("lines", self.source_color)
#cv2.waitKey(0)
# Initial Intersection class
inter = Intersections()
d = []
for i in range(0, linesP.shape[1]):
for j in range(i + 1, linesP.shape[1]):
line1 = linesP[0][i]
line2 = linesP[0][j]
# check the lines as a pair to compute their intersections
if inter.acceptLinesPPair(line1, line2, np.pi / 20.0):
intersection = inter.computeintersectP(line1, line2)
print intersection
if ((intersection[0] > 0
and intersection[0] < self.source_color.shape[1]) and
(intersection[1] > 0
and intersection[1] < self.source_color.shape[0])):
#if ((intersection[0] > 0) and (intersection[1] > 0)):
inter.append(intersection[0], intersection[1])
cv2.circle(self.source_color,
(intersection[0], intersection[1]), 10,
(0, 255, 0))
#cv2.line(self.source_color,(line1[0],line1[1]),(line1[2],line1[3]),(255, 255, 0))
#cv2.line(self.source_color,(line2[0],line2[1]),(line2[2],line2[3]),(255, 255, 0))
cv2.imwrite("images/result_images/Lines_intersection.jpg",
self.source_color)
#cv2.imshow("lines and intersections", self.source_color)
#cv2.waitKey(0)
# combination of the extracted intersection with harris corner detetction
gray = cv2.cvtColor(self.source, cv2.COLOR_BGR2GRAY)
'''harris corner detector and draw red circle around them'''
self.features = cv2.goodFeaturesToTrack(gray,
1000,
0.001,
5,
None,
None,
2,
useHarrisDetector=True,
k=0.00009)
if len(self.features.shape) == 3.0:
assert (self.features.shape[1:] == (1, 2))
self.features.shape = (self.features.shape[0], 2)
for x, y in self.features:
self.corners += [(x, y)]
inter.append_features(x, y)
cv2.circle(self.source_color, (x, y), 8, (255, 255, 255))
# subpixel accuracy
'''define the criteria to stop and refine the corners'''
criteria = (cv2.TERM_CRITERIA_MAX_ITER | cv2.TERM_CRITERIA_EPS, 100,
0.03)
#criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.001)
cv2.cornerSubPix(gray, self.features, (5, 5), (-1, -1), criteria)
for x, y in self.features:
self.refined_corners += [(x, y)]
inter.append_refined_features(x, y)
cv2.circle(self.source_color, (x, y), 4, (0, 255, 0))
'''Now draw them'''
res = np.hstack((self.corners, self.refined_corners))
res = np.int0(res)
self.source_color[res[:, 1], res[:, 0]] = [0, 0, 255]
self.source_color[res[:, 3], res[:, 2]] = [0, 255, 0]
#cv2.imwrite(str(self.IMAGE_NAME) + '_subpixel.png', self.im_new)
cv2.imwrite("images/result_images/Features and subpixel accuracy.jpg",
self.source_color)
#cv2.imshow("Features and subpixel accuracy", self.source_color)
#cv2.waitKey(0)
return inter
class Self_Drive:
def __init__(self,
speed=0.15,
lane_offset=140,
wait_period=10,
hard_coded_turns=True):
self.hard_coded_turns = hard_coded_turns
self.speed = speed
self.pid = SteeringPid(lane_offset, kp=0.1, ki=0.006, kd=1.2)
self.intersections_pid = SteeringPid(1, kp=0.1, ki=0.006, kd=1.2)
self.current_turn = None
self.current_turn_direction = None
self.handling_intersection = False
self.inter_start_time = time.time()
self.go_straight = False
self.angle = 0
self.waypoint = Waypoint()
self.car = Car_Control()
self.detector = Image_Process()
self.vs = cv2.VideoCapture(
"/dev/video2",
cv2.CAP_V4L) # TODO: figure out a good way to do this
self.intersections = Intersections(wait_period=wait_period)
self.pipeline = rs.pipeline()
self.config = rs.config()
self.config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
self.pipeline.start(self.config)
def go_to_point(self, point):
self.car.steer(0.0)
self.car.drive(self.speed)
self.waypoint.set_point(point)
while not self.waypoint.arrived():
try:
grabbed, frame = self.vs.read()
speed = self.speed
if not grabbed:
print("Couldn't Grab Next Frame")
break
offset, d2c, image = self.detector.offset_detect(frame)
angle = self.pid.run_pid(offset)
# TODO: integrate in intersection detection. It will have changed.
# at_intersection = ((d2c != None) and (d2c < 15))
# if at_intersection:
# speed = speed / 2
# at_intersection = ((d2c != None) and (d2c < 10))
# if at_intersection:
# speed = speed / 2
# at_intersection = ((d2c != None) and (d2c < 3))
# if at_intersection:
# speed = 0
# print('at intersection')
# self._handle_intersection()
self._handle_intersection()
self.car.drive(speed)
self.car.steer(angle)
except Exception as e:
print(repr(e))
print("Arrived at {}".format(point))
self.car.stop()
def self_drive(self):
global run
global image
global white
global yellow
global frame
global dst
global api_speed
global depth_colormap
global run_yolo_bool
global stop_at_light
global obstacle
self.car.steer(0.0)
if run:
self.car.drive(self.speed)
yolo_run_count = 0
while True:
try:
grabbed, frame = self.vs.read()
speed = self.speed
if not grabbed:
print("Couldn't Grab Next Frame")
break
yolo_run_count += 1
if yolo_run_count == 30:
run_yolo_bool = True
yolo_run_count = 0
offset, image, white, yellow = self.detector.offset_detect(
frame)
self.angle = self.pid.run_pid(offset)
if run and not obstacle and not stop_at_light:
if dst != (None, None):
self.waypoint.set_point(dst)
if self.waypoint.arrived():
dst = (0, 0)
run = 0
self._handle_intersection()
self.car.drive(api_speed)
self.car.steer(self.angle)
#print(self.angle)
else:
self.car.drive(0)
except Exception as e:
print(repr(e))
self.car.stop()
def _check_obstacle(self):
global obstacle
global depth_colormap
while (True):
frames = self.pipeline.wait_for_frames()
depth_frame = frames.get_depth_frame()
depth_image = np.asanyarray(depth_frame.get_data())
start = 200 #int(200 + 3 * self.angle)
end = 400 #int(400 + 3 * self.angle)
num = 0
for i in range(start, end):
#for j in range(210,230):
if depth_image[220][i] < 600 and depth_image[220][i] > 0:
num += 1
if num >= 15:
obstacle = True
else:
obstacle = False
#print(run, obstacle, stop_at_light, dst, api_speed)
time.sleep(0.25)
if obstacle:
print("Obstacle!")
depth_colormap = cv2.applyColorMap(
cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_HSV)
cv2.rectangle(depth_colormap, (start, 210), (end, 230),
(255, 0, 0), 5)
def _find_closest(self, point, turn, direction):
distance_values = distance.cdist([point], TURNS[turn][direction])
closest_index = distance_values.argmin()
if closest_index != len(distance_values) - 1:
closest_index = closest_index + 1
# print(distance_values[0][closest_index])
print(closest_index)
return TURNS[turn][direction][closest_index]
def _handle_intersection(self):
intersection, turn = self.intersections.get_intersection()
#print("intersection {}, turn {}".format(intersection, turn))
if self.handling_intersection and not self.hard_coded_turns:
cur_pos, old_pos = GPS.get_gps_all()
#des_pos = self._find_closest(cur_pos, self.current_turn, self.current_turn_direction)
#angle, _ = Translate.get_angle(cur_pos, old_pos, des_pos)
# should break us out of the intersection handling mode.
# this checks to see if the point we are closest to is the last point in the intersection
#if des_pos == TURNS[self.current_turn][self.current_turn_direction][-1]:
# self.handling_intersection = False
#self.angle = angle/2
# self.angle = self.intersections_pid.run_pid(angle)
des_pos = TURNS[self.current_turn][self.current_turn_direction][-1]
at_end = self.intersections._within_box(cur_pos, des_pos)
des_pos = TURNS[self.current_turn][self.current_turn_direction][0]
end_straight = self.intersections._within_box(
cur_pos, des_pos, 100, 100)
if at_end or self.inter_start_time + 7 < time.time():
print("ending intersection handling at ", time.time(),
"and at_end is", at_end)
print(self.inter_start_time + 5, time.time())
self.handling_intersection = False
return
if end_straight:
self.go_straight = False
if self.go_straight:
self.angle = 0
print("going straight")
return
if self.current_turn in ["2", "3", "4", "5"]:
if self.current_turn_direction == "LEFT":
self.angle = -20
elif self.current_turn_direction == "RIGHT":
self.angle = 30
elif self.current_turn_direction == "STRAIGHT":
self.car.mouse_straight(wait=8000,
duration=20000,
angle=0,
speed=(self.speed + 0.3))
elif self.current_turn_direction == "LEFT":
self.angle = -20
elif self.current_turn_direction == "RIGHT":
self.angle = 30
#print("Cur Pos: {}, Old Pos: {}, Des Pos; {}".format(cur_pos, old_pos, des_pos))
#print("Turn angle: {}".format(self.angle))
elif intersection:
self.handling_intersection = True
self.current_turn = turn
self.inter_start_time = time.time()
self.go_straight = True
if self.current_turn in ["2", "3", "4", "5"]:
self.current_turn_direction = self.waypoint.get_turn()
elif self.current_turn in ["0", "7"]:
self.current_turn_direction = "LEFT"
self.car.stop()
time.sleep(1)
elif self.current_turn in ["1", "6"]:
self.current_turn_direction = "RIGHT"
self.car.stop()
time.sleep(1)
print("Turning {} at {} at {}".format(self.current_turn_direction,
self.current_turn,
self.inter_start_time))
if self.hard_coded_turns:
self.handling_intersection = False
if self.current_turn in ["2", "3", "4", "5"
]: #at the four way, tune seperately
if self.current_turn_direction == "RIGHT":
self.car.right(wait=0.9,
duration=2.5,
angle=30,
speed=(self.speed + 0.3))
elif self.current_turn_direction == "LEFT":
self.car.left(wait=0 + 1.8,
duration=1.8,
angle=-20,
speed=(self.speed + 0.3))
elif self.current_turn_direction == "STRAIGHT":
self.car.straight(wait=0 + .75,
duration=1.75,
angle=0,
speed=(self.speed + 0.34))
elif self.current_turn_direction == "RIGHT":
self.car.right(wait=0.0 + .1,
duration=2,
angle=30,
speed=(self.speed + 0.34))
elif self.current_turn_direction == "LEFT":
self.car.left(wait=0.2 + 1,
duration=2,
angle=-20,
speed=(self.speed + 0.34))
def _handle_intersection_old(self):
is_intersection, action, coor = self.intersections.get_intersection()
if not is_intersection:
return
raise Exception(
"Intersections: {} {}: Is not at an intersection!".format(
coor, action))
print("At {} Intersection!".format(action))
which_turn = action
if action == "FOUR_WAY":
action = self.waypoint.get_turn()
print("Driving {}".format(action))
if action == "STRAIGHT":
self.car.straight(wait=0 + .75,
duration=1.75,
angle=0,
speed=(self.speed + 0.3))
elif action == "LEFT":
self.car.steer(0)
if which_turn != "FOUR_WAY":
self.car.stop()
time.sleep(1)
if which_turn == "FOUR_WAY":
#self.car.straight(wait=.25, duration=.25, angle=-10, speed=(self.speed + 0.4))
self.car.left(wait=0 + 1,
duration=1.8,
angle=-20,
speed=(self.speed + 0.3))
else:
self.car.left(wait=0.1 + 1,
duration=2,
angle=-20,
speed=(self.speed + 0.3))
elif action == "RIGHT":
self.car.steer(0)
if which_turn != "FOUR_WAY":
self.car.stop()
time.sleep(1)
if which_turn == "FOUR_WAY":
print("here")
#self.car.straight(wait=.2, duration=0.25, angle=20, speed=(self.speed + 0.4))
self.car.right(wait=0.0 + .9,
duration=2.5,
angle=30,
speed=(self.speed + 0.3))
else:
self.car.right(wait=0.0 + .1,
duration=1.6,
angle=30,
speed=(self.speed + 0.3))
print("Done Driving {}".format(action))
