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))
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)