결과
자율주행모드(Q)로 주행하니 커브길을 다음과 같이 SLIDE WINDOW로 차선을 인식하여 갈 수 있다.
특히 커브길에는 avg_val이 0이 되도록 PID 제어기에서 P GAIN이 동작한다.
여기서는 -0.1를 게인으로 설정했다.
커브 길을 지난 다음에는 차선 정 가운데에 위치하기 위해 dist_steer를 0으로 만들도록 코드가 작동한다.
server.py
openCV 가 아닌 ROS 자체에서 트랙바 기능을 코드로 구현
dynamic reconfigure
cfg : 트랙바에 추가할 내용
e_stop.py -> lidar_e_stop.cfg으로 값이 들어간다.
라
lidar2D로 바꿔서 출력하라
scan:=lidar2D
steering_publisher.py
바퀴로 차선 인식하는 코드
lidar.py
라이다로 차선 인식하는 코드
17. Lane Detection어 강의 교재
1. 기본 용어
픽셀
해상도:픽셀의 수
프레임: 동영상의 화면 하나 하나
FPS: 영상이 바뀌는 속도 , 화면의 부드러움
2. 차선 인식 과정
1) 차선 추출
ROI(REGION OF INTEREST)를 우선 정해서 필요한 영역을 지정한다.
2) Color Space Conversion
RGB, HSV, HLS, YUV 등 이미지의 스페이스를 선택한다.
H : 색상
S : 채도 (높을 수록 원색에 가까고 낮을 수록 흰색에 가깝다.) - 흰색이나 검은색(무채색)은 어둡게, 하늘색, 노란색은 밝게 나온다.
V : 밝기 (높을 수록 밝은 초록색이 된다.) RGB와 가장 가까운 결과가 나오며 어두우면 어둡게 밝으면 밝게 나온다.
3) Image Threshold(Binary Image)
이미지를 이진화 시켜서 0-255 -> 0/1로만 바꾼다. 차선은 1, 그 외는 0으로 만들어 버린다.
4) Perspective Transform(Bird Eye View)
원근법을 일직선으로 바꾼다. 위쪽에서 차선을 본 듯한 이미지로 변경한다.
5) Sliding Window (옵션 항목)
곡선의 차선 추출
특정 Window를 정해서 순차적으로 이미지를 탐색하는 방식을 통해 차선 검출
이진화된 이미지 상에서 파란 창이 위에서부터 쭉 탐색해서 흰색 부분이 일정 이상이면 차선 부분만을 검출한다.
Window의 크기나, 한번에 몇 개의 윈도우가 지나가도록 할 지 결정
3. 차선 기반의 주행을 하는 방법
1) 차선을 검출한다.
(1) 거리 계산
Bird Eye View에서 왼, 오른쪽 차선과 차의 중심 선(카메라 렌즈의 기준)의 거리를 픽셀 단위로 계산할 수 있다.로주
(2) 기울기 계산
중심선을 0도로 두고 양쪽 차선의 기울기 계산
(3) 거리값과 기울기값을 이용하여 제어
왼쪽 거리와 오른쪽 거리의 부호를 반대로 두어
더했을 때 0이 되면 중앙이므로 주행하도록 한다.
기울기값도 (왼쪽 기울기 + 오른쪽 기울기) = 0 이 되도록 해야한다.
예를 들면 왼쪽 기울기가 30도 오른쪽 기울기가 50도라면 40도 정도 회전하도록 해야한다.
다이나믹 컨피규어
srv = Server(image_processingConfig, self.configure_cb)
시뮬레이션에서의 이미지를 받아온다.
rospy.Subscriber("/image_jpeg/compressed", CompressedImage, self.Image_CB)
self.frame_pub = rospy.Publisher("/wecar/frame", Image, queue_size=1)
self.bev_pub = rospy.Publisher("/wecar/bev_frame", Image, queue_size=1)
self.color_detect_pub = rospy.Publisher("/wecar/color_detected_frame", Image, queue_size=1)
self.bev_result_pub = rospy.Publisher("/wecar/bev_result_frame", Image, queue_size=1)
self.result_pub = rospy.Publisher("/wecar/result_frame", Image, queue_size=1)
self.steering_pub = rospy.Publisher("/wecar/steering_angle", Float32, queue_size=5)각 이미지 변환 과정을 보여준다.
/wecar/steering_angle 은 angular.z를 의미한다.
점 4개를 찍기
def __init__(self):
self.TARGET_PTS = np.float32([[0, 0], [640, 0], [0, 480], [640, 480]])
def configure_cb(self, config, level):
self.ORIGINAL_PTS = np.float32([[config.left_upper_x_pos, config.left_upper_y_pos],\
[config.right_upper_x_pos, config.right_upper_y_pos],\
[config.left_lower_x_pos, config.left_lower_y_pos],\
[config.right_lower_x_pos, config.right_lower_y_pos]])각 4개의 점을 (x, y)로 찍어낸다.
여기에선 HLS를 사용한다.
self.YELLOW_LANE_LOW = np.array([config.yellow_h_low, config.yellow_l_low, config.yellow_s_low])
self.YELLOW_LANE_HIGH = np.array([config.yellow_h_high, config.yellow_l_high, config.yellow_s_high])
self.WHITE_LANE_LOW = np.array([config.white_h_low, config.white_l_low, config.white_s_low])
self.WHITE_LANE_HIGH = np.array([config.white_h_high, config.white_l_high, config.white_s_high])
여기까지 실시간으로 변경되는 다이나믹 reconfigure 값이다.
실습 시작
시뮬레이션을 연다.
kw-cobot@kwcobot-HGDT-Series:~$ roslaunch scout_sim_launch simulater.launch
1. 차선 일직선화 하기
플러그인 - 이미지뷰 - compressed(왼쪽)
플로그인 - 이미지뷰 - frame(오른쪽)
플로그인 - configuration - dynamic reconfigure (아래)
위 처럼 환경을 조성해준다.
이렇게 차선을 일직선으로 펴준다.
이후 위 코드에 값을 넣어준다.
lane_detection.launch
<?xml version="1.0"?>
<launch>
<node name="steering_node" pkg="lane_detection" type="steering_publisher.py" output="screen">
<rosparam file="$(find lane_detection)/params/lane_detection.yaml" command="load" />
</node>
<node name="object_detector" pkg="lane_detection" type="e_stop.py" output="log">
<rosparam file="$(find lane_detection)/params/e_stop.yaml" command="load" />
</node>
<!-- <node name="control_wecar" pkg="lane_detection" type="control_wecar.py" output="screen">
<rosparam file="$(find lane_detection)/params/control.yaml" command="load" />
</node> -->
</launch>주석 처리 해준다.
불러오면서 차선의 일직선화가 실행되도록 한다.
kw-cobot@kwcobot-HGDT-Series:~$ roslaunch lane_detection lane_detection.launch
HLV 설정 기존에 설정했으므로 변경할 필요 없다.
2. 이미지 콜백함수
이미지 타입 변경해주기
frame = self.cvbridge.compressed_imgmsg_to_cv2(img, "bgr8")오픈시브이에서 처리하는 이미지를 ROS에서 처리 가능하도록 바꿔준다.
BEV = BIRD EYE VIEW
def bird_eye(self, frame):
result = cv2.warpPerspective(frame, self.matrix, (640, 480))
return result프레임을 matrix를 가지고 640x480 이미지로 변경해준다. 탑 뷰 이미지 로 변환해준다.
def configure_cb(self, config, level):
self.ORIGINAL_PTS = np.float32([[config.left_upper_x_pos, config.left_upper_y_pos],\
[config.right_upper_x_pos, config.right_upper_y_pos],\
[config.left_lower_x_pos, config.left_lower_y_pos],\
[config.right_lower_x_pos, config.right_lower_y_pos]])
self.YELLOW_LANE_LOW = np.array([config.yellow_h_low, config.yellow_l_low, config.yellow_s_low])
self.YELLOW_LANE_HIGH = np.array([config.yellow_h_high, config.yellow_l_high, config.yellow_s_high])
self.WHITE_LANE_LOW = np.array([config.white_h_low, config.white_l_low, config.white_s_low])
self.WHITE_LANE_HIGH = np.array([config.white_h_high, config.white_l_high, config.white_s_high])
self.matrix = cv2.getPerspectiveTransform(self.ORIGINAL_PTS, self.TARGET_PTS)
self.inv_matrix = cv2.getPerspectiveTransform(self.TARGET_PTS, self.ORIGINAL_PTS)
self.REF_LINE_X = config.ref_line_x
return config
color_detected_frame
def color_detect(self, img):
# print(self.YELLOW_LANE_LOW)
# print(self.YELLOW_LANE_HIGH)
hls = cv2.cvtColor(img, cv2.COLOR_BGR2HLS)
mask_white = cv2.inRange(hls, self.WHITE_LANE_LOW, self.WHITE_LANE_HIGH)
mask_yellow = cv2.inRange(hls, self.YELLOW_LANE_LOW, self.YELLOW_LANE_HIGH)
mask = cv2.bitwise_or(mask_white, mask_yellow)
# cv2.imshow("hls", hls)
# cv2.imshow("mask_yellow", mask_yellow)
# cv2.imshow("mask_white", mask_white)
# cv2.imshow("h", hls[:,:,0])
# cv2.imshow("s", hls[:,:,1])
# cv2.imshow("v", hls[:,:,2])
# cv2.waitKey(1)
# return mask_white
# return mask_yellow
return mask하얀색과 노란색을 모두 검출되도록 한다.
슬라이딩 작업
bev_result_frame
def sliding_left(self, img):
left_list = []
'''
row: starting from y=179 to y=460, moving by 40
col: starting from x=19 to x=300, moving by 5
'''
for j in range(179, img.shape[0] - 20, 40):
j_list = []
for i in range(19, int(img.shape[1]/2) - 20, 5):
num_sum = np.sum(img[j - 19:j + 21, i - 19:i + 21]) #window size is 20*20
if num_sum > 100000: #pick (i,j) where its num_sum is over 100000
j_list.append(i)
try:
len_list = []
#cluster if a gap between elements in the list is over 5
result = np.split(j_list, np.where(np.diff(j_list) > 5)[0] + 1)
for k in range(0, len(result)):
len_list.append(len(result[k])) #append the lengths of each cluster
largest_integer = max(len_list)
for l in range(0, len(result)):
if len(result[l]) == largest_integer:
avg = int(np.sum(result[l]) / len(result[l])) #average
left_list.append((avg, j)) #avg points of left side
except:
continue
return left_list100000 = 255 * 400을 의미한다.
40 * 40 * 255 중에서 400 *255만 흰색이면 윈도우 상에 표시하는 코드
self.sliding_list_left = self.sliding_left(colored_image)
self.sliding_list_right = self.sliding_right(colored_image)
print("left : {}".format(self.sliding_list_left))
print("right : {}".format(self.sliding_list_right))
터미널을 보면 슬라이딩 점이 출력된다.
left : [(126, 179), (126, 219), (126, 259), (126, 299), (121, 339), (121, 379), (121, 419), (121, 459)]
right : [(485, 179), (485, 219), (487, 259), (487, 299), (490, 339), (490, 379), (490, 419), (490, 459)]
제어해주기산
거리 계산
def steering(self, frame, sliding_list_left, sliding_list_right):
x_left = []
x_right = []
self.REF_LINE_X = 320
if len(sliding_list_left) == 0:
left_distance = 0
left_x = 0
else:
left_x = sliding_list_left[-1][0]
left_distance = self.REF_LINE_X - sliding_list_left[-1][0] #should be positive
# print("slidng_lisjlklljkljkljkljkljkljkljljukljkljkljklkjlt_left[-1][0] : {}".format(sliding_list_left[-1][0]))
if len(sliding_list_right) == 0:
right_distance = 0
right_x = 0
else:
right_x = sliding_list_right[-1][0]
right_distance = self.REF_LINE_X - sliding_list_right[-1][0] #should be negative
# print("sliding_list_right[-1][0] : {}".format(sliding_list_right[-1][0]))
# print("left_distance : {}".format(left_distance))
# print("right_distance : {}".format(right_distance))
#extract x coordinates from sliding_list_left if len(sliding_list_left) == 0:
left_distance = 0
left_x = 0차선이 안 잡히면 0만큼 이동해라
맨 아래에 있는, 즉 가장 차에 가까이 있는 점을 [-1][0]으로 처리
레퍼런스 라인을 기준으로 왼쪽, 오른쪽을 나눴다.
distance 두 값을 더하면 0이 되도록 제어한다.
기울기 계산
left_diff_arr = np.diff(x_left)
divider = len(left_diff_arr)
if divider == 0:
divider = 1
left_diff_sum = np.sum(left_diff_arr)
left_avg = int(left_diff_sum/divider)
# print("left_diff_arr{}".format(left_diff_arr))
# print("left_diff_sum{}".format(left_diff_sum))
# print("left_avg{}".format(left_avg))
#remove inappropriate cases
for i in range(0, len(left_diff_arr)):
if (left_diff_arr[i] > (left_avg +80)) or (left_diff_arr[i] < (left_avg - 80)): #the value 80 can be modified
self.sliding_list_left = []
left_diff_sum = 0왼쪽 기울기값의 평균와 오른쪽 기울기 값의 평균을 구한다.
#remove inappropriate cases
for i in range(0, len(left_diff_arr)):
if (left_diff_arr[i] > (left_avg +80)) or (left_diff_arr[i] < (left_avg - 80)): #the value 80 can be modified
self.sliding_list_left = []
left_diff_sum = 0차선은 연속적이므로 평균 기울기에 대해 편차가 크지 않아야 한다.
즉, 차선 기울기에 너무 큰 변화량이 있으면 버리겠다는 코드이다.
if left_distance == 0 and right_distance != 0:
# dist_steer = right_distance - right_dist_th
dist_steer = 150왼쪽 차선은 안 보이고, 오른쪽 차선이 보일 때 핸들을 꺾도록 제어한다.
왼, 오른쪽 차선 모두 보이는 일반적인 상황일 때,
else:
dist_steer = right_distance + left_distance
avg_val = float((left_diff_sum + right_diff_sum)/2) *-1
if avg_val > 100:
avg_val = 100
elif avg_val < -100:
avg_val = -100
avg_val /= 100
avg_val *= 15
print("avg_val : {}".format(avg_val * (-0.6)))
print("dist_steer : {}".format(dist_steer))
self.steering_pub.publish(avg_val*(-1.2) + dist_steer * 0.07)
# self.steering_pub.publish(avg_val*(-0.5))
# self.steering_pub.publish(dist_steer * 0.06)
return frame #return frame is for visualization왼, 오 각각의 기울기 평균값의 평균값을 구한다.
if avg_val > 100:
avg_val = 100
elif avg_val < -100:
avg_val = -100범위를 벗어나지 않도록 정리해준다.
Result_frame
윈도우를 원을 이용하여 점으로 찍어주기.
# 2.list points
for i, pos in enumerate(self.sliding_list_left):
cv2.circle(bird_eye_image, pos, 5,(255,i*20,0), -1)
for i, pos in enumerate(self.sliding_list_right):
cv2.circle(bird_eye_image, pos, 5,(i*20,255,0), -1)
avg_val /= 100
avg_val *= 15
print("avg_val : {}".format(avg_val * (-0.6)))dist_steer 음수이면 왼쪽 차선과 가까운 상태, 양수이면 오른쪽 차선과 가까운 상태이다
양수 --> 오른쪽 차선과 가까운 상태 --> 좌회전이 필요 (angualr.z가 양수)
음수 --> 왼쪽 차선과 가까운 상태 --> 우회전이 필요
avg_val은 우회전이 필요할 때 음수, 좌회전이 필요할 때는 양수가 나온다.
좌회전 -- > angular.z 양수, avg_val 음수
kw-cobot@kwcobot-HGDT-Series:~$ rostopic echo /wer/steering_angle
---
data: 2.02999997139
---
data: 2.02999997139
---
data: 2.02999997139
---
data: 2.02999997139
---
data: 2.02999997139
---
data: 2.02999997139
---
data: 2.02999997139
---
data: 2.02999997139
---
data: 2.02999997139
★ steering_angle을 구독해서 cmd_vel을 발행하는 코드 짜기
controller.py
#! /usr/bin/env python
# -*- coding: utf-8 -*-
# /wecar/steering_angle Topic Subscribe (std_msgs/Float32)
# /cmd_vel Topic Publish (geometry_msgs/Twist)
# steering_angle을 구독해서 cmd_velf로 발행하는 코드 짜기
## /wecar/steering_angle --> Twist.angular.z
## Twist.angular.z --> /wecar/steering_angle
import rospy
from geometry_msgs.msg import Twist
from std_msgs.msg import Float32
class RobotController(object):
def __init__(self):
rospy.Subscriber("wecar/steering_angle", Float32, self.steering_angle_callback)
self.drive_pub = rospy.Publisher("cmd_vel", Twist, queue_size=1)
rospy.Timer(rospy.Duration(0.1), self.timer_callback)
def steering_angle_callback(self, data):
self.z_data = data.data
# twist_data = Twist()
# twist_data.linear.x = 0.3
# twist_data.angular.z = data.data
# self.drive_pub.publish(twist_data)
# rospy.loginfo("data published")
def timer_callback(self, event):
twist_data = Twist()
twist_data.linear.x = 0.3
twist_data.angular.z = self.z_data
self.drive_pub.publish(twist_data)
rospy.loginfo("{} data published".format(self.z_data))
def run():
rospy.init_node("robot_controller")
robot_controller= RobotController()
rospy.spin()
if __name__ == "__main__":
run()
result_frame
다시 원본 형태로 바꿔준다.
자동차 제어하기 위한 튜닝 작업
print("avg_val : {}".format(avg_val))
print("dist_steer : {}".format(dist_steer))
self.steering_pub.publish(avg_val*(-0.012) + dist_steer * 0.01)
# self.steering_pub.publish(avg_val*(-0.5))
# self.steering_pub.publish(dist_steer * -0.001)
return frame #return frame is for visualization
controller.py에서
steering_publisher.py의 streering_angle값을 받아 cmd_vel의 augular.z 값을 변경시킨다.
즉, 시뮬레이터 상의 핸들이 자동으로 차선을 따라 가게 방향을 조정하게 해준다.
rospy.loginfo("{} data published".format(self.z_data))
속도를 올리면 gain값을 줄이는 방향으로 튜닝해야 한다.
차선이 두 개가 문제가 생김
-> 차선 한 쪽이 bev_result_frame 상에서 없어지지 않게 해야한다.
#self.TARGET_PTS = np.float32([[0, 0], [640, 0], [0, 480], [640, 480]])
self.TARGET_PTS = np.float32([[n, m], [640-n, m], [n, 480-m], [640-n, 480-m]])으로 바꿔주고
코너 부분에서 avg_vel의 gain 값을 높여준다.
속도 2.0부터는 힘들다.
카메라 프레임이 속도가 못 따라가는 건 아닐까 싶다. 센서의 문제라고 생각한다.
터미널 상황
kw-cobot@kwcobot-HGDT-Series:~$ roslaunch lane_detection lane_detection.launch kw-cobot@kwcobot-HGDT-Series:~/catkin_ws/src/lane_detection/scripts$ rosrun lane_detection controller.py kw-cobot@kwcobot-HGDT-Series:~$ rqt
kw-cobot@kwcobot-HGDT-Series:~$ roslaunch scout_sim_launch simulater.launch
결과물
steering_publisher.py
self.steering_pub.publish(avg_val*(-0.1) + dist_steer * 0.001)
'Robot > ROS' 카테고리의 다른 글
| 20강 ROS SLAM (0) | 2022.03.16 |
|---|---|
| 민 - ar_maker (0) | 2022.03.15 |
| 민 - ar_maker (0) | 2022.03.14 |
| 민 - [20강] (0) | 2022.03.14 |
| 킴 [이미지 처리] (0) | 2022.03.10 |
