def scan_front(degree_s, degree_e, dist):
global lidar_range
while not detect_stoplineB(image):
for degree in range(degree_s, degree_e): # scan degree
if lidar_range[degree] <= dist:
print(lidar_range[degree])
drive(0, 0)
drive(c, 3)
def avoid_obstacles():
global limit, lidar_range, flag, c # left = 0', right = 180'
while not detect_stoplineB(image):
for degree in range(44, 190):
# Obstacles in front of vehicle -> turn left
if flag == 0 and 44 < degree < 134 and lidar_range[degree] <= 0.75:
flag = 1
#limit = 1
print('Turn Left')
print(lidar_range[degree], degree)
drive(c, 3)
elif flag == 1 and 175 < degree < 189 and 0.45 < lidar_range[
degree] < 0.8:
flag = 2
#limit = 2
print('Turn Right')
print(lidar_range[degree], degree)
drive(-40, 2)
elif flag == 2 and 185 < degree < 189 and 0.1 < lidar_range[
degree] < 0.3:
flag = 3
#limit = 3
time_s = time.time()
print(start)
print('Go Straight')
print(lidar_range[degree], degree)
drive(50, 2)
elif flag == 3:
time_e = time.time()
if time_e - time_s >= 6:
#limit = 4
drive(-7, 2)
def start():
global pub
global image
global Gap
global Width, Height
global c
pid_P = 0.7
pid_I = 0
pid_D = 0
sum_angle = 0
mission = 3
prev_angle = 0
rospy.init_node('auto_drive')
pub = rospy.Publisher('xycar_motor', xycar_motor, queue_size=1)
image_sub = rospy.Subscriber("/usb_cam/image_raw", Image, img_callback)
lidar_sub = rospy.Subscriber("/scan",
LaserScan,
lidar_callback,
queue_size=1)
print("---------- Xycar A2 v1.1.0 ----------")
rospy.sleep(3)
mission_start = time.time()
print('START!!!!')
while True:
while not image.size == (640 * 480 * 3):
continue
lpos, rpos = process_image(image)
center = (lpos + rpos) / 2
angle = -(Width / 2 - center)
sum_angle += angle
diff_angle = angle - prev_angle
c = pid_P * angle + pid_I * sum_angle + pid_D * (diff_angle)
prev_angle = angle
# PID Control
#prev_angle = angle
#drive(angle, 12)
print('Mission', mission)
#drive(c, 3)
if mission == 0:
#if scan_front(50, 130, 0.4) and light_detection(image):
if not light_detection(image) and detect_stoplineB(image):
drive(0, 0) # detect stop line, not green light
rospy.sleep(1)
else:
#if light_detection(image):
# drive(c, 6)
drive(c, 6)
if time.time() - mission_start >= 30:
mission = 1
mission_start = time.time()
elif mission == 1:
if not detect_stoplineB(image):
drive(c, 3)
else:
drive(0, 0)
if time.time() - mission_start >= 10:
mission = 2
mission_start = time.time()
elif mission == 2:
scan_front(0, 134, 0.5)
# if scan_front(0, 134, 0.5):
# drive(c, 3)
# else:
# drive(0, 0)
if detect_stoplineB(image):
mission = 3
elif mission == 3:
avoid_obstacles()
# if limit == 0:
# drive(c, 3)
# elif limit == 1:
# drive(-40, 2)
# elif limit == 2:
# drive(50, 2)
# elif limit == 3:
# drive(20, 2)
# elif limit == 4:
# drive(-7, 2)
if not light_detection(image):
drive(0, 0)
mission = 4
elif mission == 4:
if not light_detection(image) and limit < 5:
drive(0, 0)
if limit == 5:
drive(-40, 3) # turn left
mission = 5
elif limit == 6:
drive(40, 3) # turn right
mission = 5
elif mission == 5:
if not detect_stoplineB(image):
drive(c, 3)
else:
drive(0, 0)
return
#if not detect_stopline(image):
#if light_detection(image):
#if scan_front(44, 134):
# drive(0, 3)
#else:
# drive(0, 0)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
rospy.spin()
def start():
global pub
global image
global Gap
global Width, Height
global c, mission
pid_P = 0.7
pid_I = 0
pid_D = 0
sum_angle = 0
mission = 0
cnt = 0
prev_angle = 0
rospy.init_node('auto_drive')
pub = rospy.Publisher('xycar_motor', xycar_motor, queue_size=1)
image_sub = rospy.Subscriber("/usb_cam/image_raw", Image, img_callback)
lidar_sub = rospy.Subscriber("/scan",
LaserScan,
lidar_callback,
queue_size=1)
print("---------- Xycar A2 v1.1.0 ----------")
rospy.sleep(4)
rate = rospy.Rate(10)
print('START!!!!')
mission_start = 0
while True:
while not image.size == (640 * 480 * 3):
continue
lpos, rpos = process_image(image)
center = (lpos + rpos) / 2
angle = -(Width / 2 - center)
sum_angle += angle
diff_angle = angle - prev_angle
c = pid_P * angle + pid_I * sum_angle + pid_D * (diff_angle)
prev_angle = angle
# PID Control
# prev_angle = angle
# drive(angle, 12)
if mission == 0:
if not light_detection_3(image):
drive(0, 0) # detect light, not green light
rospy.sleep(4.5)
print('----------Light detected and sleep----------------')
entry_rotary(1, 1.5)
else:
drive(c, 6)
elif mission == 1:
if detect_stoplineB(image):
print('--------Stopline detected and sleep-----------')
drive(0, 0)
rospy.sleep(4.5)
flag_ = True
drive_time = time.time() + 5
if time.time() > drive_time:
flag_ = False
if flag_:
drive(c, 6)
else:
cnt += 1
if cnt == 1:
entry_rotary(1, 1)
mission_start = time.time()
elif cnt == 2:
entry_rotary(2, 1.5)
else:
drive(c, 6)
elif mission == 2:
if detect_stoplineB(image):
print('-----------Change mission 3----------------')
mission = 3
else:
print('-----------In Rotary----------------')
drive(c, 4)
elif mission == 3:
drive(c, 3)
avoid_obstaclesB()
elif mission == 4:
if not light_detection_4(image):
drive(0, 0)
rospy.sleep(4.5)
else:
drive(c, 3)
elif mission == 5:
if limit == 5:
print('main left')
#turn_left()
elif limit == 6:
print('main right')
#turn_right()
elif mission == 6:
if not scan_front(45, 135, 0.3):
drive(c, 3)
else: # wall detect
print('--------------------------------END')
drive(0, 0)
rospy.sleep(100)
rate.sleep()
if cv2.waitKey(1) & 0xFF == ord('q'):
break
rospy.spin()