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