def __init__(self, mission_number, database): # 초기화
self.__mission_number = mission_number
self.database = database
self.Lane_detect = Lane_Detection(poly_order=1)
self.radius = 5 # 5m to see
self.map = [(0, 0)]
self.target = (0, 0)
'''
def __init__(self, db, num):
self.num = num
self.db = db
self.flag = db.flag
self.lane = Lane_Detection()
self.packet = [0, None, None, None, None]
self.__path_thread = threading.Thread(target=self.__main)
#path가 이미 만들어져 있다면 True 아니면 Fasle
self.make_path = False
#정지신호
self.stop = False
self.yolo = YOLO(db=db)
self.idx = 0
class Mapping: # 나중에 이 함수에 display 메소드도 추가해야 할듯..?
def __init__(self, mission_number, database): # 초기화
self.__mission_number = mission_number
self.database = database
self.Lane_detect = Lane_Detection(poly_order=2)
self.radius = 5 # 5m to see
self.map = [(0, 0)]
self.target = (0, 0)
self.obstacle = []
def update_map(self, left_on=True, right_on=True, lidar_on=True, img=None):
#img = cv2.imread(self.database.main_cam.data)
self.Lane_detect.run(img) #self.database.main_cam.data
ob = [(1000, 1000)]
if left_on:
left_x = (self.Lane_detect.left.allx) #/37 * 20
left_y = (600 - self.Lane_detect.left.ally) #/35 * 20
left = np.polyfit(left_y, left_x, 2)
left_ob = [(np.polyval(left, i), i) for i in range(600)]
if right_on:
right_x = (self.Lane_detect.right.allx) #/37 * 20
right_y = (600 - self.Lane_detect.right.ally) #/35 * 20
right = np.polyfit(right_y, right_x, 2)
right_ob = [(np.polyval(right, i), i) for i in range(600)]
#ob = left_ob + right_ob
if lidar_on:
'''lidar = [5645, 5642, 5635, 5643, 5622, 5630, 5643, 5642, 5640, 5658, 5634, 5634, 5657, 5671, 5660, 5667, 5675, 5687, 5703, 5697, 5688, 5702, 5713, 5724, 5730, 5743, 5764, 5775, 5789, 5778, 5797, 5804, 5826, 5835, 5851, 5875, 5871, 5876, 5892, 5907, 5942, 5959, 5976, 6002, 6019, 6025, 6043, 6073, 6098, 6121, 6139, 6184, 6186, 6220, 6237, 6274, 6296, 6324, 6341, 6378, 6409, 6445, 6466, 6507, 6533, 6565, 6594, 6646, 6676, 6709, 6763, 6794, 6831, 6887, 6927, 6959, 7011, 7046, 7107, 7166, 7202, 7261, 7310, 7349, 7422, 7478, 7454, 7378, 7398, 7446, 7515, 7575, 7778, 8001, 8083, 8145, 8232, 8292, 8392, 8472, 8534, 8624, 8725, 8807, 8894, 9004, 9095, 9218, 9312, 9412, 9519, 9634, 9745, 9883, 10005, 10156, 10275, 10429, 10558, 10699, 10860, 11029, 11153, 11332, 11502, 11653, 11833, 12049, 12234, 9900, 9862, 9827, 9772, 9746, 9704, 9683, 9648, 9622, 1000000, 1000000, 12750, 10612, 12203, 11673, 12208, 12306, 12352, 15484, 15436, 15555, 15676, 15754, 15994, 16313, 16923, 22700,
22645, 22133, 22106, 7735, 7756, 7736, 7694, 7708, 7719, 7686, 7681, 7697, 1000000, 1000000, 39561, 39528, 39522, 1000000, 1000000, 1000000, 1000000, 1000000, 1000000, 1000000, 1000000, 1000000, 1000000, 1000000, 1000000, 1000000, 1000000, 1000000, 1000000, 1000000, 1000000, 1000000, 1000000, 1000000, 1000000, 1000000, 1000000, 1000000, 1000000, 1000000, 1000000, 16534, 15390, 15021, 15051, 15099, 15128, 15044, 14681, 14640, 14609, 16723, 1000000, 1000000, 1000000, 1000000, 18237, 3247, 3235, 3237, 3247, 3312, 3287, 3245, 3223, 3212, 3204, 3236, 3253, 3289, 3334, 3321, 3332, 3326, 3377, 7194, 1000000, 13527, 13484, 13683, 13713, 13737, 13577, 13375, 13195, 13000, 12830, 12649, 12481, 12311, 12156, 12008, 8971, 8847, 5590, 5561, 5520, 5497, 5385, 5714, 5432, 5255, 5296, 5396, 5252, 5012, 5142, 5236, 5393, 4768, 4810, 5495, 5859, 4873, 4681, 5072, 4821, 5117, 4752, 4750, 4330, 4457, 4478, 4672, 4675, 4522, 4492, 4671, 4544, 4440, 4293, 4322, 4510, 4305, 4577, 4537, 4530,
4396, 4532, 4546, 4461, 4329, 4373, 4396, 4127, 3638, 4063, 4042, 3931, 3986, 4276, 4507, 4518, 4485, 4566, 4406, 4576, 4188, 4348, 4148, 4254, 4479, 4492, 3999, 3794, 3734, 3531, 3504, 4345, 4437, 4154, 3702, 3672, 3665, 3670, 3828, 3474, 3496, 4086, 3265, 3263, 3510, 3570, 3441, 3276, 3642, 3990, 4095, 4082, 4083, 4044, 3508, 3281, 3291, 3783, 3334, 3380, 3589, 3472, 3442, 3234]
'''
lidar = self.database.lidar.data
lidar = np.array(lidar)
lidar[lidar > 15000] = 0
angle = (np.linspace(0, 180, 361) / 180 * np.pi)
x = x_scale * np.cos(angle) * lidar / 1000 + 400
y = y_scale * np.sin(angle) * lidar / 1000 + 0
#plt.plot(x,y)
#plt.show()
for z in zip(x, y):
ob.append(z)
self.obstacle += ob
return self.obstacle
class Path_plan:
def __init__(self, db, num):
self.num = num
self.db = db
self.flag = db.flag
self.lane = Lane_Detection()
self.packet = [0, None, None, None, None]
self.__path_thread = threading.Thread(target=self.__main)
#path가 이미 만들어져 있다면 True 아니면 Fasle
self.make_path = False
#정지신호
self.stop = False
self.yolo = YOLO(db=db)
self.idx = 0
def __main(self):
#본선
if (self.num == 2):
while not self.flag.system_stop:
if (self.idx >= 655 and self.idx <= 765): #1
self.__static_obstacle()
elif ((self.idx >= 407 and self.idx <= 430)
or (self.idx >= 986 and self.idx <= 1025)
or (self.idx >= 1145 and self.idx <= 1170)): #6
self.__signal_straight()
elif ((self.idx >= 496 and self.idx <= 530)
or (self.idx >= 766 and self.idx <= 805)
or (self.idx >= 1490 and self.idx <= 1530)
or (self.idx >= 1721 and self.idx <= 1770)
or (self.idx >= 1830 and self.idx <= 1875)): #7
self.__static_obstacle()
elif (self.idx >= 86 and self.idx <= 120): #8
self.__parking()
else:
self.__path_tracking()
############### MISSON ###############
## 0. 직선 주행(기본 주행)
def __path_tracking(self):
self.packet = [0, None, None, None, None]
#True일 경우 빨강
#self.packet[4] = self.lane.get_floor_color(self.db.sub_cam.data)
#print('now default')
## 1. 정적 장애물 미션
def __static_obstacle(self):
self.packet = [1, None, None, None, None]
## 2. 동적 장애물 미션
def __dynamic_obstacle(self):
self.packet = [2, None, None, None, None]
self.packet[2] = self.distance()
self.packet[4] = self.lane.get_floor_color(self.db.sub_cam.data)
## 3. 비신호 직진 미션
def __non_signal_straight(self):
self.make_path = False
self.packet = [3, None, None, None, None]
## 4. 비신호 좌회전 미션
def __non_signal_left(self):
self.make_path = False
self.packet = [4, None, None, None, None]
## 5. 비신호 우회전 미션
def __non_signal_right(self):
self.make_path = False
self.packet = [5, None, None, None, None]
## 6. 신호 좌회전 미션
def __signal_left(self):
self.packet[0] = 6
self.packet[2] = False
self.packet[3] = True
while self.packet[2] != True: #정지선을 인식할 때 까지 전진
time.sleep(0.01)
temp_img = make_binary(self.db.sub_cam.data, (800, 600))
self.packet[2] = self.lane.get_stop_line(temp_img)
#print(self.packet[2])
cv2.destroyAllWindows()
light = self.yolo.check_lights()
if (light == 'red'): # 좌회전신호 판단 >> 주행 가능: 1, 멈춤: 0
self.packet[3] = False
while light == 'red':
light = self.yolo.check_lights()
time.sleep(0.01)
self.packet[3] = True
## 7. 신호 직진 미션
def __signal_straight(self):
self.packet[0] = 7
self.packet[2] = False
self.packet[3] = True
while self.packet[2] != True: #정지선을 인식할 때 까지 전진
time.sleep(0.01)
temp_img = make_binary(self.db.sub_cam.data, (800, 600))
self.packet[2] = self.lane.get_stop_line(temp_img)
cv2.destroyAllWindows()
light = self.yolo.check_lights()
if (light != 'green'): # 적색 신호 판단 >> 주행 가능: 1, 멈춤: 0
self.packet[3] = False
while light != 'green':
light = self.yolo.check_lights()
time.sleep(0.01)
self.packet[3] = True
## 8. 주차 미션
def __parking(self):
self.packet[0] = 8
self.packet[1] = None
if (self.make_path == False):
if self.idx >= 86 and self.idx <= 92:
if parking(self.db.sub_cam.data, [0, 0, 0, 0, 0]) == 1:
cv2.destroyAllWindows()
self.packet[1] = 1
self.make_path = True
elif self.idx >= 93 and self.idx <= 98:
if parking(self.db.sub_cam.data, [0, 0, 0, 0, 0]) == 1:
cv2.destroyAllWindows()
self.packet[1] = 2
self.make_path = True
elif self.idx >= 99 and self.idx <= 103:
if parking(self.db.sub_cam.data, [0, 0, 0, 0, 0]) == 1:
cv2.destroyAllWindows()
self.packet[1] = 3
self.make_path = True
elif self.idx >= 104 and self.idx <= 110:
if parking(self.db.sub_cam.data, [0, 0, 0, 0, 0]) == 1:
cv2.destroyAllWindows()
self.packet[1] = 4
self.make_path = True
elif self.idx >= 111 and self.idx <= 115:
if parking(self.db.sub_cam.data, [0, 0, 0, 0, 0]) == 1:
cv2.destroyAllWindows()
self.packet[1] = 5
self.make_path = True
elif self.idx >= 116 and self.idx <= 120:
if parking(self.db.sub_cam.data, [0, 0, 0, 0, 0]) == 1:
cv2.destroyAllWindows()
self.packet[1] = 6
self.make_path = True
#####################################
def set_mission_idx(self, idx):
self.idx = idx
def get_mission_packet(self):
return self.packet
def distance(self):
lidar_raw_data = self.db.lidar.data
minimum_distance = lidar_raw_data[180] / 10
min_theta = 180
car_width = 120 # cm
for theta in range(90, 270):
if (minimum_distance > lidar_raw_data[theta] / 10) and \
((lidar_raw_data[theta] / 10) * abs(np.cos(theta * np.pi / 360)) < (
car_width / 2)): # 2 Conditions
minimum_distance = lidar_raw_data[theta] / 10
min_theta = theta
distance = minimum_distance * np.sin(min_theta * np.pi / 360)
return distance
def start(self):
print("Starting Path_plan thread...")
self.__path_thread.start()
time.sleep(1)
print("Path_plan thread start!")
self.yolo.start()
def join(self):
print("Terminating Path_plan thread...")
self.__path_thread.join()
time.sleep(0.1)
print("Path_plan termination complete!")
self.yolo.join()
class Combine: # 나중에 이 함수에 display 메소드도 추가해야 할듯..?
def __init__(self, mission_number, database): # 초기화
self.__mission_number = mission_number
self.database = database
self.Lane_detect = Lane_Detection()
self.radius = 5 # 5m to see
self.map = [(0, 0)]
self.target = (0, 0)
'''
if self.__mission_num == 0: self.__static_obstacle()
elif self.__mission_num == 1: self.__dynamic_obstacle()
elif self.__mission_num == 2: self.__parking()
'''
def update_map(self):
self.Lane_detect.run(self.database.main_cam.data)
left_x = self.Lane_detect.left.allx * 0.1
right_x = self.Lane_detect.right.allx * 0.1
left_y = self.Lane_detect.left.ally * 0.1
right_y = self.Lane_detect.right.ally * 0.1
left = np.polyfit(left_y, left_x, 2)
right = np.polyfit(right_y, right_x, 2)
line_left = np.poly1d(left)
line_right = np.poly1d(right)
line = np.linspace(0, 100, 100)
ob = [(int(line_left(i)), i)
for i in range(80)] + [(int(line_right(i)), i)
for i in range(80)]
'''
plt.plot(line_left(line),line)
plt.plot(line_right(line),line)
plt.show()
'''
lidar = self.database.lidar.data
lidar[lidar > 10000] = 0
print(lidar)
angle = (np.linspace(0, 180, 361) / 180 * np.pi)
x = -x_scale * np.cos(angle) * lidar / 1000
y = y_scale * np.sin(angle) * lidar / 1000
plt.plot(x, y)
plt.show()
for z in zip(x, y):
ob.append(z)
return ob
'''
calibration 필요 --> pixel 하나당 몇 m 인지
lm per 480 pixel
'''
########## 각 상황에 맞게 Lidar, Lane_Detection 이용하여 함수 짜기 ##########
## 신호/비신호는 path를 짜는것에 있어서는 같을 것 같아 하나로 묶음
## 각 상황에 맞는 map과 local target값을 넣으면
def __static_obstacle(self):
print(0)
# map, target 만들기
def __dynamic_obstacle(self):
print(0)
# map, target 만들기
def __parking(self):
'''
Mission: 차량이 있는 주차공간과 장애인 전용 주차구간을 피해 주차를 함
<Flow Chart>
Lidar의 135도 방향중 일정 거리 이내에 ┌ 물체가 포착됨(이미 주차되어있는 차량이 있는 경우) -> return 0
└ 포착되지 않음 -> 주차용 카메라에서 사각형을 찾음 ┌ 사각형이 검출되지 않는 경우 -> return 0
└ 사각형이 검출되는 경우 ┌ 장애인 주차구역인 경우 -> return 0
└ 장애인 주차구역이 아닌경우 -> target, map을 저장, return 1
'''
'''
# ==================== lidar에 장애물이 검출되나 확인 ====================== #
database = Database(0, 0, 1, 1, 0)
database.start()
lidar_array = database.lidar
if lidar_array[135] < LiDAR_MIN: # lidar data의 135도에 물체가 있을 경우 그냥 넘어감
self.map = None
self.target = None
return 0
# ================================================================== #
'''
video = cv2.VideoCapture('./video/real_park1.mp4')
while (video.isOpened()):
_, cap = video.read()
cv2.waitKey(10)
cap = cv2.cvtColor(cap, cv2.COLOR_BGR2GRAY)
cv2.imshow('original', cap)
# cap = cv2.resize(cap, dsize = (0, 0), fx = 0.5, fy = 0.5, interpolation=cv2.INTER_LINEAR) # 가로세로 크기 조절
cap = cv2.resize(cap,
dsize=(640, 480),
interpolation=cv2.INTER_AREA)
height = cap.shape[0]
width = cap.shape[1]
# ============================ 사각형 찾기 ============================== #
## bird view로 바꿈
source_points = np.array([(0.4 * width, 0.15 * height),
(0.1 * width, 0.45 * height),
(0.8 * width, 0.55 * height),
(0.75 * width, 0.17 * height)],
np.float32) # 왼쪽 위부터 반시계방향
destination_points = np.array([(0.3 * width, 0),
(0.3 * width, height),
(0.8 * width, height),
(0.8 * width, 0)],
np.float32) # 왼쪽 위부터 반시계방향
warp_matrix = cv2.getPerspectiveTransform(source_points,
destination_points)
birdview_img = cv2.warpPerspective(cap,
warp_matrix, (width, height),
flags=cv2.INTER_LINEAR)
cv2.imshow('why', birdview_img)
## 차선만 뽑아냄(흑백으로 바꿈)
_, threshold_img = cv2.threshold(birdview_img, 180, 255,
cv2.THRESH_BINARY)
## FOR DEBUGGING ##
# 디버깅시 차선의 흑백 구분이 제대로 되나 확인
# cv2.imshow('threshold_img', threshold_img)
cv2.imshow('threshold_img', threshold_img)
## 사각형이 검출되지 않는 경우 -> 그냥 진행
contours, _ = cv2.findContours(threshold_img, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
is_rectangle = 0 # 사각형이 있는 경우 1, 없는 경우 0
rectangle_coordinate = [(0, 0), (0, 0), (0, 0),
(0, 0)] # 사각형이 있는 경우 꼭지점의 네 좌표를 저장
center = [(0, 0)]
for cnt in contours:
#approx = cv2.approxPolyDP(cnt, 0.01 * cv2.arcLength(cnt, True), True)
approx = cv2.approxPolyDP(cnt, 20, True)
cv2.drawContours(birdview_img, [approx], 0, (0), 5)
if len(approx) == 4:
is_rectangle = 1
## 무게중심 구하기
raw_center = cv2.moments(approx)
center = (int(raw_center['m10'] / raw_center['m00']),
int(raw_center['m01'] / raw_center['m00']))
## 좌표 찾아 집어넣기(rectangle_coordinate에 네개의 꼭짓점을 각각 집어넣음)
rectangle_coordinate[0] = (approx.ravel()[0],
approx.ravel()[1])
rectangle_coordinate[1] = (approx.ravel()[2],
approx.ravel()[3])
rectangle_coordinate[2] = (approx.ravel()[4],
approx.ravel()[5])
rectangle_coordinate[3] = (approx.ravel()[6],
approx.ravel()[7])
break
# ================================================================== #
## FOR DEBUGGING ##
# 디버깅시 사각형 제대로 찾아지나 확인
new_img = cv2.circle(threshold_img, rectangle_coordinate[0], 20,
(255, 255, 255), -1)
new_img = cv2.circle(threshold_img, rectangle_coordinate[1], 20,
(255, 255, 255), -1)
new_img = cv2.circle(threshold_img, rectangle_coordinate[2], 20,
(255, 255, 255), -1)
new_img = cv2.circle(threshold_img, rectangle_coordinate[3], 20,
(255, 255, 255), -1)
cv2.imshow("detect_rectangle", new_img)
# ======================== 사각형이 없는경우 ========================== # >> 사각형이 없는 경우는 그냥 넘어감
## 장애인 구역인지 판단
if is_rectangle == 0:
self.map = None
self.target = None
continue
handicap_radius = int(
math.sqrt((rectangle_coordinate[0][0] - center[0]) *
(rectangle_coordinate[0][0] - center[0]) +
((rectangle_coordinate[0][1] - center[1]) *
(rectangle_coordinate[0][1] - center[1]))) / 6)
if 2 * handicap_radius < 50: is_rectangle = 0
if is_rectangle == 0:
self.map = None
self.target = None
continue
# ================================================================== #
# ======================== 사각형이 있는 경우 ========================= # >> 사각형이 있는 경우 장애인 주차구간인지 아닌지 판단
count_white = 0
count_all = handicap_radius * handicap_radius * 4
for i in range(center[0] - handicap_radius,
center[0] + handicap_radius, 1):
for j in range(center[1] - handicap_radius,
center[1] + handicap_radius, 1):
if threshold_img[j][i] == 255:
count_white = count_white + 1
## 장애인 주차구간일때 >> 장애인 주차구간인 경우 그냥 넘어감
if count_white / count_all > 0.1: # 흰색 비율이 무게중심 주위 정사각형의 10% 이상 차지할때는 장애인표시 있는걸로 생각
self.map = None
self.target = None
continue
## 장애인 주차구간이 아닐때 >> target과 map을 저장
else: # 흰색 비율이 무게중심 주위 정사각형의 10% 미만일떄는 장애인표시 없는걸로 생각
## 주차공간의 사각형 중 차에서 가장 가까운 두 꼭짓점을 찾음
close_find = 0
far_find = 0
close_point = [0, 1]
far_point = [2, 3]
for i in range(0, 4, 1):
if rectangle_coordinate[i][1] > center[1]:
close_point[close_find] = i
close_find = close_find + 1
else:
far_point[far_find] = i
far_find = far_find + 1
if (rectangle_coordinate[close_point[0]][0] - rectangle_coordinate[close_point[1]][0])\
* (rectangle_coordinate[far_point[0]][0] - rectangle_coordinate[far_point[1]][0]) < 0: # close_point[0]과 far_point[0]이 center를 기준으로 대각선에 있지 않도록 조절
temp = close_point[0]
close_point[0] = close_point[1]
close_point[1] = temp
print(rectangle_coordinate)
# 모두 0으로 되어 있는 빈 Canvas(검정색)
img = np.zeros((height, width, 3), np.uint8)
new_map = cv2.line(img, rectangle_coordinate[far_point[0]],
rectangle_coordinate[far_point[1]],
(255, 255, 255), 10) # 선은 흰색을 따라 그림
new_map = cv2.line(new_map, rectangle_coordinate[far_point[0]],
rectangle_coordinate[close_point[0]],
(255, 255, 255), 10) # 선은 흰색을 따라 그림
new_map = cv2.line(new_map, rectangle_coordinate[far_point[1]],
rectangle_coordinate[close_point[1]],
(255, 255, 255), 10) # 선은 흰색을 따라 그림
new_map = cv2.circle(new_map, center, 10, (255, 255, 255), -1)
#self.map = new_map
#self.target = center
## FOR DEBUGGING ##
# map 출력 테스트
cv2.imshow('final', new_map)
class Combine: # 나중에 이 함수에 display 메소드도 추가해야 할듯..?
def __init__(self, mission_number, database): # 초기화
self.__mission_number = mission_number
self.database = database
self.Lane_detect = Lane_Detection(poly_order=1)
self.radius = 5 # 5m to see
self.map = [(0, 0)]
self.target = (0, 0)
'''
if self.__mission_num == 0: self.__static_obstacle()
elif self.__mission_num == 1: self.__dynamic_obstacle()
elif self.__mission_num == 2: self.__parking()
'''
def update_map(self):
img = cv2.imread("./video/pre_lane_Moment.jpg")
self.Lane_detect.run(img) #self.database.main_cam.data
left_x = (self.Lane_detect.left.allx) #/37 * 20
right_x = (self.Lane_detect.right.allx) #/37 * 20
#대칭이동 (y= 400 에서)
left_y = (600 - self.Lane_detect.left.ally) #/35 * 20
right_y = (600 - self.Lane_detect.right.ally) #/35 * 20
left = np.polyfit(left_y, left_x, 2)
right = np.polyfit(right_y, right_x, 2)
line_left = np.poly1d(left)
line_right = np.poly1d(right)
ob = [(int(line_left(i)), i)
for i in range(800)] + [(int(line_right(i)), i)
for i in range(800)]
'''
plt.plot(line_left(line),line)
plt.plot(line_right(line),line)
plt.show()
'''
lidar = [
5645, 5642, 5635, 5643, 5622, 5630, 5643, 5642, 5640, 5658, 5634,
5634, 5657, 5671, 5660, 5667, 5675, 5687, 5703, 5697, 5688, 5702,
5713, 5724, 5730, 5743, 5764, 5775, 5789, 5778, 5797, 5804, 5826,
5835, 5851, 5875, 5871, 5876, 5892, 5907, 5942, 5959, 5976, 6002,
6019, 6025, 6043, 6073, 6098, 6121, 6139, 6184, 6186, 6220, 6237,
6274, 6296, 6324, 6341, 6378, 6409, 6445, 6466, 6507, 6533, 6565,
6594, 6646, 6676, 6709, 6763, 6794, 6831, 6887, 6927, 6959, 7011,
7046, 7107, 7166, 7202, 7261, 7310, 7349, 7422, 7478, 7454, 7378,
7398, 7446, 7515, 7575, 7778, 8001, 8083, 8145, 8232, 8292, 8392,
8472, 8534, 8624, 8725, 8807, 8894, 9004, 9095, 9218, 9312, 9412,
9519, 9634, 9745, 9883, 10005, 10156, 10275, 10429, 10558, 10699,
10860, 11029, 11153, 11332, 11502, 11653, 11833, 12049, 12234,
9900, 9862, 9827, 9772, 9746, 9704, 9683, 9648, 9622, 1000000,
1000000, 12750, 10612, 12203, 11673, 12208, 12306, 12352, 15484,
15436, 15555, 15676, 15754, 15994, 16313, 16923, 22700, 22645,
22133, 22106, 7735, 7756, 7736, 7694, 7708, 7719, 7686, 7681, 7697,
1000000, 1000000, 39561, 39528, 39522, 1000000, 1000000, 1000000,
1000000, 1000000, 1000000, 1000000, 1000000, 1000000, 1000000,
1000000, 1000000, 1000000, 1000000, 1000000, 1000000, 1000000,
1000000, 1000000, 1000000, 1000000, 1000000, 1000000, 1000000,
1000000, 1000000, 1000000, 1000000, 16534, 15390, 15021, 15051,
15099, 15128, 15044, 14681, 14640, 14609, 16723, 1000000, 1000000,
1000000, 1000000, 18237, 3247, 3235, 3237, 3247, 3312, 3287, 3245,
3223, 3212, 3204, 3236, 3253, 3289, 3334, 3321, 3332, 3326, 3377,
7194, 1000000, 13527, 13484, 13683, 13713, 13737, 13577, 13375,
13195, 13000, 12830, 12649, 12481, 12311, 12156, 12008, 8971, 8847,
5590, 5561, 5520, 5497, 5385, 5714, 5432, 5255, 5296, 5396, 5252,
5012, 5142, 5236, 5393, 4768, 4810, 5495, 5859, 4873, 4681, 5072,
4821, 5117, 4752, 4750, 4330, 4457, 4478, 4672, 4675, 4522, 4492,
4671, 4544, 4440, 4293, 4322, 4510, 4305, 4577, 4537, 4530, 4396,
4532, 4546, 4461, 4329, 4373, 4396, 4127, 3638, 4063, 4042, 3931,
3986, 4276, 4507, 4518, 4485, 4566, 4406, 4576, 4188, 4348, 4148,
4254, 4479, 4492, 3999, 3794, 3734, 3531, 3504, 4345, 4437, 4154,
3702, 3672, 3665, 3670, 3828, 3474, 3496, 4086, 3265, 3263, 3510,
3570, 3441, 3276, 3642, 3990, 4095, 4082, 4083, 4044, 3508, 3281,
3291, 3783, 3334, 3380, 3589, 3472, 3442, 3234
] #self.database.lidar.data
lidar = np.array(lidar)
lidar[lidar > 15000] = 0
angle = (np.linspace(0, 180, 361) / 180 * np.pi)
x = x_scale * np.cos(angle) * lidar / 1000 + 400
y = y_scale * np.sin(angle) * lidar / 1000 + 50
plt.plot(x, y)
plt.show()
for z in zip(x, y):
ob.append(z)
return ob
'''
calibration 필요 --> pixel 하나당 몇 m 인지
lm per 480 pixel
'''
########## 각 상황에 맞게 Lidar, Lane_Detection 이용하여 함수 짜기 ##########
## 신호/비신호는 path를 짜는것에 있어서는 같을 것 같아 하나로 묶음
## 각 상황에 맞는 map과 local target값을 넣으면
def __static_obstacle(self):
print(0)
# map, target 만들기
def __dynamic_obstacle(self):
print(0)
# map, target 만들기
def __parking(self):
print(0)