1. 전체적인 제어 알고리즘
2. 차선 인식(Hough 변환, PID 제어)
Xycar가 자율주행할 때, 전방의 카메라로 양 옆의 흰색 차선을 실시간으로 인식한다. 이때 차선 인식은 Hough 변환 알고리즘을 사용하였다. 차선 인식은 전체 코스에서 지속적으로 바뀌는 차선을 계산해야 하므로 while문 루프 안에서 Hough 변환 차선 인식 함수가 매 루프마다 호출되는 방식이다.
process_image→ return lpos, rpos
process_image 함수 내에서 graycolor 변환 → GaussianBlur 처리 → Canny 변환→ HoughLinesP 로 직선 추출 → divide_left_right 함수를 통해 좌측 차선, 우측 차선 구별→ get_line_pos 함수를 통해 좌우 차선 프레임 위치 결정
Hough 변환 알고리즘은 컴퓨터 비전과 이미지 처리에서 특히 직선이나 원과 같은 형태를 탐지하는 데 널리 사용되는 기법이다. 작성한 코드에서는 Hough 변환 알고리즘이 차선 탐지를 위해 사용되었다. 코드의 흐름은 process_image 함수가 호출되며 시작되고 함수 내에서는 7단계로 이루어져 차선 인식을 계산한다.
1.
process_image 함수는 카메라로부터 캡처된 차량의 전방 이미지를 인자로 받는다.
2.
받은 이미지는 Grayscale로 변환된다. 이는 컬러 이미지를 흑백으로 바꾸어 이미지 처리를 단순화하고, 컴퓨터 비전 알고리즘이 노이즈와 불필요한 정보를 줄여 더욱 효율적으로 작동하도록 한다.
3.
Grayscale 이미지에는 GaussianBlur 처리가 이루어진다. 이 과정은 이미지에서 노이즈와 세부사항을 제거하여 이미지를 부드럽게 만들며, 이를 통해 Canny 엣지 탐지기가 중요한 세부사항을 더 잘 강조할 수 있게 한다.
4.
블러링된 이미지에 Canny Edge Detection이 적용된다. 이 알고리즘은 이미지의 선명한 경계를 탐지하고 이를 강조하여 그 결과를 이진 이미지로 변환한다.
5.
이진 이미지는 HoughLinesP 함수에 전달되어 이미지에서 직선을 탐지한다. 이 함수는 Hough 변환 알고리즘을 기반으로 한 Probabilistic Hough Transform을 사용하여 이미지 내의 직선들을 탐지하고 이를 반환한다.
6.
divide_left_right 함수를 통해 HoughLinesP에서 반환된 모든 직선들을 좌측 차선과 우측 차선으로 분류한다. 이는 각각의 직선의 기울기를 계산하고 그 값에 따라 분류하는 것으로 이루어진다.
7.
get_line_pos 함수를 통해 각각의 차선에 대해 그 위치를 결정하게 된다. 이는 차선의 평균 기울기와 y절편을 계산함으로써 가능하다.
Main 함수 내 while문에서 차선 인식 계산
while (hough_flag == True):
...
lpos, rpos = process_image(image)
center = (lpos + rpos) / 2
angle = PID(center, 0.50, 0.0, 0.0)
...
drive(angle, speed)
...
Python
복사
hough_flag는 Xycar가 전체 코스의 시작부터 막다른 길에 도달하기까지 실행되는 반복문이다. 막다른 길 AR Tag를 인식하면 hough_flag가 False로 바뀌고 다음 단계로 진입하는 방식이다. while문 안에서는 매 루프마다 lpos, rpos를 계산하여 angle 값을 결정한다.
process_image() 함수는 웹캠 등으로부터 받아온 이미지를 처리하고, 그 이미지에서 차선을 찾아내는 기능을 수행한다. 입력된 이미지는 먼저 그레이스케일로 변환되어 처리의 복잡성이 줄어들고, 그 후 가우시안 블러를 통해 노이즈와 세부적인 정보를 제거하여 대체적인 형태를 분명하게 한다. 캐니 엣지 디텍션을 통해 이미지에서 경계선을 추출하고, 이를 바탕으로 Hough 변환을 사용해 차선이 될 가능성이 있는 직선들을 찾아낸다. 찾아낸 직선들은 좌우 차선으로 분류되고, 각 차선의 중심 위치를 구하는 등의 처리 과정을 거친다. 이 과정에서 차선을 구성하는 점들을 선으로 이어 그림으로써 차선의 형태를 더욱 분명히 하고, 차량의 위치를 나타내는 사각형을 그려 시각적인 참고 정보를 제공한다.
def process_image(frame):
global Width
global Offset, Gap
global pub_temp
# frame: 웹캠 등으로부터 받아온 이미지를 말함.
# 이미지를 그레이스케일로 변환한다. 이를 통해 이미지 처리의 복잡성을 줄인다.
gray = cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY)
# 가우시안 블러를 이용해 그레이스케일 이미지를 흐릿하게 만든다. 이렇게 하면 노이즈와 세부적인 정보를 줄일 수 있다.
kernel_size = 5
blur_gray = cv2.GaussianBlur(gray,(kernel_size, kernel_size), 0)
# 캐니 엣지를 이용해 이미지에서 경계선을 찾는다. 이를 통해 차선과 같은 중요한 정보를 강조한다.
low_threshold = 60
high_threshold = 70
edge_img = cv2.Canny(np.uint8(blur_gray), low_threshold, high_threshold)
# 차선을 찾기 위해 관심 영역을 설정하고, HoughLinesP 함수를 이용해 이 영역에서 직선을 찾는다.
roi = edge_img[Offset : Offset+Gap, 0 : Width]
all_lines = cv2.HoughLinesP(roi,1,math.pi/180,30,30,10)
# 찾아낸 모든 선을 좌측 차선과 우측 차선으로 분류한다. 만약 차선이 없다면 해당 함수는 0과 640을 반환하며 종료한다.
if all_lines is None:
return 0, 640
left_lines, right_lines = divide_left_right(all_lines)
# 각 차선의 중심 위치를 구한다.
frame, lpos = get_line_pos(frame, left_lines, left=True)
frame, rpos = get_line_pos(frame, right_lines, right=True)
# 차선을 그린다.
frame = draw_lines(frame, left_lines)
frame = draw_lines(frame, right_lines)
# 차량의 위치를 나타내는 사각형을 그린다.
frame = draw_rectangle(frame, lpos, rpos, offset=Offset)
frame = cv2.rectangle(frame, (150,350 ),(500,400),(0, 0, 255), 2)
# 변환된 이미지를 OpenCV의 imshow 함수를 통해 화면에 출력한다.
bridge = CvBridge()
image_msg = bridge.cv2_to_imgmsg(frame, encoding="bgr8")
image_msg.header.frame_id = "map"
pub_temp.publish(image_msg)
return lpos, rpos
Python
복사
divide_left_right() , get_line_params(), get_line_pos() 세 가지 함수는 process_image() 내에서 호출되는 함수들로 이미지 내의 차선을 찾아내는 역할을 수행한다.
첫 번째 함수 divide_left_right는 Hough 변환을 통해 찾아낸 모든 선을 기울기를 기반으로 좌측과 우측 차선으로 분류한다.
두 번째 함수 get_line_params는 분류된 선들의 평균 기울기와 y절편을 계산한다. 이는 차선의 전반적인 경향을 파악하는 데 사용된다.
마지막 함수 get_line_pos는 차선의 평균 위치를 계산하고, 이를 이미지 위에 그린다. 이 위치 정보는 차량의 조향을 결정하는 데 사용된다. 이렇게 계산된 선들은 process_image 함수에 의해 이미지 위에 그려지며, 이는 최종적으로 차량이 어떻게 조향해야 하는지를 결정하는 데 사용된다.
# lines: HoughLinesP로부터 얻은 직선의 리스트.
# 선들을 좌측 선들과 우측 선들로 분류하는 함수이다.
def divide_left_right(lines):
global Width
low_slope_threshold = 0
high_slope_threshold = 10
# 선의 기울기를 계산하고, 기울기에 따라 선들을 필터링한다.
slopes = []
new_lines = []
for line in lines:
x1, y1, x2, y2 = line[0]
if x2 - x1 == 0:
slope = 0
else:
slope = float(y2-y1) / float(x2-x1)
if abs(slope) > low_slope_threshold and abs(slope) < high_slope_threshold:
slopes.append(slope)
new_lines.append(line[0])
# 선들을 좌측과 우측으로 분류한다.
left_lines = []
right_lines = []
for j in range(len(slopes)):
Line = new_lines[j]
slope = slopes[j]
x1, y1, x2, y2 = Line
if (slope < 0) and (x1+x2 < Width):
left_lines.append([Line.tolist()])
elif (slope > 0) and (x1 < x2):
right_lines.append([Line.tolist()])
return left_lines, right_lines
# lines: 분류된 직선의 리스트.
# 선들의 평균 기울기와 y절편을 반환하는 함수이다.
def get_line_params(lines):
# x, y, m의 합계를 초기화한다.
x_sum = 0.0
y_sum = 0.0
m_sum = 0.0
size = len(lines)
if size == 0:
return 0, 0
for line in lines:
x1, y1, x2, y2 = line[0]
x_sum += x1 + x2
y_sum += y1 + y2
m_sum += float(y2 - y1) / float(x2 - x1)
x_avg = x_sum / (size * 2)
y_avg = y_sum / (size * 2)
m = m_sum / size
b = y_avg - m * x_avg
return m, b
# img: 원본 이미지, lines: 분류된 직선의 리스트, left: 좌측 선 여부, right: 우측 선 여부
# 선들의 평균 위치를 계산하고, 이를 이미지 위에 그린 후, 이미지와 위치를 반환하는 함수이다.
def get_line_pos(img, lines, left=False, right=False):
global Width, Height
global Offset, Gap
m, b = get_line_params(lines)
if m == 0 and b == 0:
if left:
pos = 0
if right:
pos = Width
else:
y = Gap / 2
pos = (y - b) / m
b += Offset
x1 = (Height - b) / float(m)
x2 = ((Height/2) - b) / float(m)
cv2.line(img, (int(x1), Height), (int(x2), (Height/2)), (255, 0,0), 3)
return img, int(pos)
Python
복사
차선 주행 단계에서는 PID 제어 알고리즘을 적용하여 조향 품질을 향상시켰다. 이 알고리즘의 도입은 보다 부드럽고 안정적인 주행을 가능하게 만들어, 전반적인 차량의 운전 성능을 높였다. 아래 코드는 PID 제어 코드이다.
def PID(input_data, kp, ki, kd):
global start_time, end_time, prev_error, i_error
end_time = time.time()
dt = end_time - start_time
start_time = end_time
error = 320 - input_data
derror = error - prev_error
p_error = kp * error
i_error = i_error + ki * error * dt
d_error = kd * derror / dt
output = p_error + i_error + d_error
prev_error = error
if output > 50:
output = 50
elif output < -50:
output = -50
return -output
Python
복사
kp, ki, kd의 값은 각각 0.5, 0.0, 0.0을 할당했다. 해당 파라미터는 실험을 통해 정했다. 만약 조향각이 50도를 넘어가면 50도로 고정해주는 코드를 추가해 강건한 조향제어를 하도록 설계했다.
3. 장애물 회피
첫번째 미션인 장애물 회피 코스는 case1과 case2로 나뉜다. Case1은 주행시 왼쪽 장애물 회피 후 오른쪽 장애물을 회피해야하는 경우이고, 반대로 case2는 오른쪽 장애물 회피 후 왼쪽 장애물을 회피해야 한다. 두 가지 상황에 대해 여러번 실험을 통해 파라미터 수정을 거듭하여 최적화 단계를 거쳤고, 이를 위해 case에 대한 각 상황에 실행되는 코드를 달리했다. 장애물 drive_obstacle ()함수의 인자는 “right” 혹은 “left” 가 있다. Lidar로부터 LaserScan message를 받고 정면 방향180도를 기준으로 한다. 50~180 각도에서 받은 distance값을 계산하여 왼쪽의 장애물 유무를 판단하고 181~310 각도에 받은 distacne값을 계산하여 오른쪽의 장애물 유무를 판단한다. 차량에 가까워진 distance의 threshold를 0.28로 설정하여 왼쪽과 오른쪽 방향의 장애물 인식 추정값을 매 루프마다 계산하고 drive_avoid()함수를 호출하게 된다. 또한 첫번째 장애물 블록과 두번째 블록 인식 시점에 조향각 설정을 경험적으로 최적화하기 위해 각 경우의 수에 따라 방향 전환 동작 시간을 달리하였다. 두 장애물 블록 통과한 이후에는 더 이상 Lidar를 쓰지 않으므로 ob_flag를 설정하여 False에서 True로 변경하여 이후 코스에서 장애물 인식 코드를 호출되지 않도록 하였다. 아래 코드는 main 함수의 while 반복문 내에 실행되는 장애물 인식 코드이다.
left_obstacle = sum(1 for i in distance[start:180] if i < 0.28)
right_obstacle = sum(1 for i in distance[181 : end] if i < 0.28)
if left_obstacle > 5: # have to move right
drive_avoid("right")
obstacle_order = "second"
pub_ob.publish(True)
elif right_obstacle > 5: # have to move left
drive_avoid("left")
obstacle_order = "second"
pub_ob.publish(True)
else:
pub_ob.publish(False)
Python
복사
drive_avoid() 함수는 자율주행 차량이 장애물을 감지했을 때 이를 피해가는 동작을 수행한다. 동작의 세부 내용은 장애물이 감지된 순서와 위치에 따라 달라진다. 방향은 왼쪽과 오른쪽 중 하나를 선택하며, 이는 장애물이 감지된 방향에 따라 결정된다. 각 단계마다 차량의 속도와 조향 각도를 조정하여 장애물을 회피하고, 장애물을 피한 후에는 순서를 변경하여 다음 장애물에 대비한다.
# direction: 'left' 또는 'right' 문자열. 피해야 하는 방향을 나타냄.
def drive_avoid(direction):
global motor_msg, pub, right_obstacle, left_obstacle, obstacle_order
motor_msg.speed = 50 # 장애물 피하기 동작 시 보다 느린 속도로 설정
current_time = time.time() # 현재 시간 측정. 이후 시간 경과 계산에 사용.
# 장애물이 감지된 순서에 따라 다르게 처리
if obstacle_order == 'first':
if(direction == 'left'): # 장애물이 왼쪽에 있는 경우
print("turn_left")
# 현재 시간부터 0.37초 동안 차량을 오른쪽으로 조향
while(time.time() - current_time < 0.37 ):
motor_msg.angle = -40
pub.publish(motor_msg)
current_time = time.time()
# 다음 1초 동안 차량을 왼쪽으로 조향
while(time.time() - current_time < 1.0 ):
motor_msg.angle = 40
pub.publish(motor_msg)
obstacle_order = 'second' # 장애물을 피한 후 순서를 'second'로 변경
print(obstacle_order)
elif(direction == 'right'): # 장애물이 오른쪽에 있는 경우
print("turn_right")
# 현재 시간부터 0.36초 동안 차량을 왼쪽으로 조향
while(time.time() - current_time < 0.36 ):
motor_msg.angle = 50
obstacle_order = 'second' # 장애물을 피한 후 순서를 'second'로 변경
print(obstacle_order)
pub.publish(motor_msg)
else:
if(direction == 'left'): # 장애물이 왼쪽에 있는 경우
print("turn_left")
# 현재 시간부터 0.5초 동안 차량을 오른쪽으로 조향
while(time.time() - current_time < 0.5 ):
motor_msg.angle = -40
pub.publish(motor_msg)
current_time = time.time()
# 다음 1초 동안 차량을 왼쪽으로 조향
while(time.time() - current_time < 1.0 ):
motor_msg.angle = 40
pub.publish(motor_msg)
obstacle_order = 'second' # 장애물을 피한 후 순서를 'second'로 변경
elif(direction == 'right'): # 장애물이 오른쪽에 있는 경우
print("turn_right")
# 차량을 왼쪽으로 조향
motor_msg.angle = 40
obstacle_order = 'second' # 장애물을 피한 후 순서를 'second'로 변경
pub.publish(motor_msg)
Python
복사
4. 신호등 인식
장애물을 피한 이후 다음 미션으로 신호등 신호를 받아 정지 여부를 판단해야 한다. Main 함수 while 반복문 내에서 ob_flag가 False로 변경된 이후 카메라로 촬영한 매 프레임 값에서 횡단보도를 탐지하는 함수(detect_crosswalk)가 호출된다.
if (ob_flag == False):
line_detect = detect_crosswalk(image)
Python
복사
detect_crosswalk() 함수는 차량의 카메라에서 가져온 이미지를 바탕으로 횡단보도를 탐지하는 역할을 한다. 먼저 이미지의 특정 부분(ROI, Region of Interest)를 선택하여 이를 그레이스케일로 변환한다. 그런 다음 이를 이진 이미지로 변환하여 흰색 픽셀의 개수를 세고, 이를 전체 픽셀 개수로 나누어 횡단보도가 존재하는지를 판단한다. 흰색 픽셀의 비율이 0.1 이상인 경우, 즉 횡단보도 표시의 색상이 충분히 밝아 흰색에 가까운 경우에만 횡단보도가 있다고 판단한다.
# 횡단보도 탐지 함수
# image: 차량 카메라에서 가져온 이미지
def detect_crosswalk(image):
roi = image[350:400, 150:500] # 원본 이미지에서 관심 영역(ROI)을 설정. 이 값은 경험적으로 찾아내야 함.
gray = cv2.cvtColor(roi, cv2.COLOR_BGR2GRAY) # ROI를 그레이스케일로 변환
_, binary = cv2.threshold(gray, 200, 255, cv2.THRESH_BINARY) # 그레이스케일 이미지를 이진 이미지로 변환. 여기서는 픽셀값 200 이상을 가진 픽셀은 255(흰색)으로, 그렇지 않은 픽셀은 0(검은색)으로 변환.
white_pixels = np.count_nonzero(binary == 255) # 이진 이미지에서 흰색 픽셀의 개수를 세기
total_pixels = binary.shape[0] * binary.shape[1] # 이진 이미지의 전체 픽셀 개수를 계산
# 흰색 픽셀의 비율이 전체 픽셀 대비 0.1보다 크다면 횡단보도로 판단하고 True를 반환
if float(white_pixels) / total_pixels > 0.1:
return True
else:
return False # 그렇지 않다면 횡단보도가 아니라고 판단하고 False를 반환
Python
복사
detect_crosswalk() 함수에서 흰색 비율이 많은 횡단보도를 인식하여 True를 반환하면 신호등 색과 남은 시간(time_count)값을 통해 상황에 따라 다른 행동을 결정한다. 신호등에 따라 결정한 속도를 계산하는 함수로 desperado() 함수를 호출하게 된다.
if line_detect and left_color == 'R' :
desperado('stop', time_count)
elif line_detect and left_color == 'G':
desperado('sprint', time_count)
elif line_detect and left_color == 'Y':
if time_count < 1.8:
desperado('hold back',time_count)
else:
desperado('sprint',time_count)
Python
복사
desperado() 함수는 횡단보도에서 신호등의 상태에 따라 자율주행 차량의 동작을 제어한다. 행동에는 'stop'(빨간불에서 정지), 'sprint'(초록불에서 전진), 'hold back'(노란불에서 정지)가 있다. 함수는 행동의 종류와 대기 시간을 입력받아 행동을 수행한다.
'stop' 동작은 차량을 정지시키고 설정된 시간 동안 대기하도록 한다. 'sprint' 동작은 차량이 횡단보도를 아직 건너지 않았다면 3초 동안 전력 주행한다. 'hold back' 동작은 차량을 정지시키고 남은 노란불 시간과 빨간불 시간 동안 대기하도록 한다.
# 횡단보도에서의 행동을 조절하는 함수
# action: 수행할 동작 (stop, sprint, hold back 중 하나)
# time_count: 대기 시간
def desperado(action, time_count):
global crosswalk_flag # 횡단보도 통과 전/후를 나타내는 전역 변수
if action == 'stop': # 'stop' 동작인 경우 (신호등이 빨간색인 경우)
drive(0, 0) # 차량을 정지
rospy.sleep(time_count+0.5) # 설정한 시간만큼 대기
crosswalk_flag = 1 # 횡단보도를 통과한 상태로 업데이트
return
elif action == 'sprint': # 'sprint' 동작인 경우
if crosswalk_flag == 0:
start_time = time.time() # 현재 시간 저장
while(time.time() - start_time < 3): # 3초 동안 전력 질주
drive(0, 6) # 차량을 전진시킴 (허브 변환하면 그냥 주행이 안전할지도)
crosswalk_flag = 1 # 횡단보도를 통과한 상태로 업데이트
return
elif action == 'hold back': # 'hold back' 동작인 경우
drive(0, 0) # 차량을 정지
rospy.sleep(time_count+10) # 설정한 시간에 10초를 더한 시간만큼 대기 (남은 노란불 + 빨간불 시간)
crosswalk_flag = 1 # 횡단보도를 통과한 상태로 업데이트
return
Python
복사
5. 갈림길
첫번째 AR 태그(ID = 2)를 인식하면 갈림길에 위치한 두 신호등의 색깔과 남은 시간을 토픽으로부터 받는다. 차량은 멈추지 않고, 향후 시점에 마주칠 횡단보도의 신호등 색깔과 남은 시간을 계산하여 왼쪽 또는 오른쪽으로 향할 결정을 내린다. 갈림길에서의 주행은 Hough 변환을 통한 자율주행이 아닌, 결정된 방향에 따라 직진하거나 방향 전환을 수행하는 동작으로 이루어진다. 이 동작은 정해진 시간 동안 수행된다. 아래 코드는 main 함수 내 while 반복문에서 실행되는 갈림길 코드이다.
if arData["ID"] == 2 and arData["DZ"] < 0.71 and ar_flag == False:
twoway_decide()
Python
복사
twoway_decide 함수는 차량이 갈림길에 도착했을 때 왼쪽 또는 오른쪽 방향을 결정하는 역할을 한다. decide_left_or_right 함수를 통해 결정된 방향에 따라 차량은 일정 시간 동안 직진하고, 결정된 방향으로 방향을 전환한 후 다시 직진하는 과정을 거친다. 이때 사용되는 시간 값은 실험을 통해 경험적으로 결정된다. 이 함수를 통해 차량은 신호등의 신호와 남은 시간을 고려하여 안전하게 갈림길 미션을 수행할 수 있다.
# 갈림길에서의 주행 방향 결정 함수
def twoway_decide():
global ar_flag # 전역 변수 ar_flag 선언
direction = decide_left_or_right(left_color,time_count) # 왼쪽 혹은 오른쪽 방향을 결정하는 함수 호출
start_intersection = time.time() # 현재 시간을 start_intersection에 저장
ar_flag = True # ar_flag를 True로 설정
# 왼쪽으로 턴을 결정한 경우
if direction == 'turn_left':
start_intersection = time.time() # 현재 시간을 start_intersection에 저장
# 일정 시간(3.5초) 동안 직진
while(time.time() - start_intersection < 3.5):
drive(0,5)
start_intersection = time.time() # 현재 시간을 start_intersection에 저장
# 일정 시간(1.2초) 동안 왼쪽으로 방향전환
while(time.time() - start_intersection < 1.2):
drive(-40,5)
start_intersection = time.time()
# 일정 시간(0.5초) 동안 다시 직진
while(time.time() - start_intersection < 0.5):
drive(40,5)
else:
print("error") # 방향 결정에 에러가 발생한 경우 에러 메시지 출력
# 오른쪽으로 턴을 결정한 경우
if direction == 'turn_right':
start_intersection = time.time() # 현재 시간을 start_intersection에 저장
# 일정 시간(2.8초) 동안 직진
while(time.time() - start_intersection < 2.8):
drive(0,5)
start_intersection = time.time() # 현재 시간을 start_intersection에 저장
# 일정 시간(1.4초) 동안 오른쪽으로 방향전환
while(time.time() - start_intersection < 1.4):
drive(40,5)
start_intersection = time.time()
# 일정 시간(0.4초) 동안 다시 직진
while(time.time() - start_intersection < 0.4):
print(3)
drive(-40,5)
else:
print("error") # 방향 결정에 에러가 발생한 경우 에러 메시지 출력
Python
복사
6. 평행 주차
Main 함수 내 while 반복문에서 갈림길 AR Tag를 인식하게 되면 마지막 미션인 평행 주차를 수행하게 된다. 아래 코드에서 loadend()를 호출하게 되고 loadend() 함수에서 hough_flag를 False로 변경하게 되고 main 함수의 while 반복문을 탈출하게 된다. 따라서 더이상 hough 변환 알고리즘을 사용하지 않게 된다. 평행 주차는 정해진 동작만 수행하므로 parking() 함수를 실행하면 된다.
elif arData["ID"] == 4 and arData["DZ"] < 0.8:
angle = loadend()
Python
복사
def loadend():
print("stop")
drive(0,0)
hough_flag = False
start_stop = time.time()
while(time.time() - start_stop < 0.5 ):
angle = 0
speed = 0
drive(angle, speed)
return angle
Python
복사
parking() 함수는 평행 주차를 수행하는 로직을 구현한 것이다. 처음 단계에서는 오른쪽으로 최대한 조향하면서 일정 시간 동안 후진한다. 그 다음 단계에서는 조향을 왼쪽으로 변경하고, 같은 시간 동안 후진한다. 마지막 단계에서는 AR 태그를 이용해 주차 위치까지의 거리를 계산하며, 거리가 충분히 가까워지면 차량을 정지시키는 로직이 구현되어 있다.
# 평행 주차를 수행하는 함수
def parking():
global start_park_pid_time # 주차 시작 시간을 나타내는 전역 변수
# 첫 번째 단계: 오른쪽으로 최대한 조향하면서 일정 시간 동안 후진
start_park = time.time() # 현재 시간 저장
while(time.time() - start_park < 1.40 ):
angle = 50 # 오른쪽으로 최대한 조향
speed = -9 # 후진
drive(angle, speed) # 차량을 제어
# 두 번째 단계: 왼쪽으로 최대한 조향하면서 같은 시간 동안 후진
start_park = time.time() # 현재 시간 저장
while(time.time() - start_park < 1.40 ):
angle = -50 # 왼쪽으로 최대한 조향
speed = -9 # 후진
drive(angle, speed) # 차량을 제어
# 세 번째 단계: 주차 AR 태그의 거리가 주차 영역까지 가까워질 때까지 전진
start_park = time.time() # 현재 시간 저장
while(time.time() - start_park < 0.8):
drive(0,0) # 차량을 정지
start_park = time.time() # 현재 시간 저장
while(time.time() - start_park < 10.8):
start_park_pid_time = time.time() # 현재 시간 저장
drive(0,9) # 차량을 전진시킴
if(arData["ID"] == 6 and arData["DZ"] < 0.74): # AR 태그의 ID가 6이고 거리가 0.74 미만인 경우
drive(0,0) # 차량을 정지
rospy.sleep(0.5) # 0.5초 동안 대기
break # 반복문을 종료
Python
복사