class AutoDrive: def __init__(self): rospy.init_node('autodrive') self.line_detector = LineDetector('/usb_cam/image_raw') # self.obstacle_detector = ObstacleDetector('/ultrasonic') self.driver = MotorDriver('/xycar_motor_msg') def trace(self): # obs_l, obs_m, obs_r = self.obstacle_detector.get_distance() self.load_drive_info() self.driver.drive(self.angle + 90, self.speed + 90) def steer(self, left, right): angle = self.line_detector.total_angle() mid = (left + right) // 2 - 320 if mid <= 30: angle = mid // 3 else: angle = mid // 1.7 angle = max(-50, angle) if angle < 0 else min(50, angle) return angle def exit(self): self.line_detector.video.release() print('finished')
class AutoDrive: def __init__(self): rospy.init_node('xycar_driver') self.line_detector = LineDetector('/usb_cam/image_raw') self.driver = MotorDriver('/xycar_motor_msg') def trace(self): angle = self.line_detector.get_left_right() angle = self.steer(angle) speed = self.accelerate(angle) self.driver.drive(angle + 90, speed + 90) def steer(self, angle): if abs(angle) < 7: angle *= 1.2 else: angle *= 3.25 angle = max(-50, angle) if angle < 0 else min(50, angle) return angle def accelerate(self, angle): if abs(angle) > 7: return 20 return 25 def exit(self): print('finished')
class AutoDrive: def __init__(self): rospy.init_node('autodrive') self.line_detector = LineDetector('/usb_cam/image_raw') self.obstacle_detector = ObstacleDetector('/ultrasonic') self.driver = MotorDriver('/xycar_motor_msg') def trace(self): # obs_l, obs_m, obs_r = self.obstacle_detector.get_distance() self.load_drive_info() self.driver.drive(self.angle + 90, self.speed + 90) def load_drive_info(self): angle = int(self.line_detector.total_angle()) abs_angle = abs(angle) print(angle, abs_angle) if abs_angle <= 2: self.angle = 0.1 elif abs_angle <= 10: self.angle = 0.2 elif abs_angle <= 14: self.angle = 2.5 else: self.angle = 20 self.angle = int(self.angle * 25) if angle < 0: self.angle *= -1 self.speed = 25 def exit(self): print('finished')
class AutoDrive: def __init__(self): rospy.init_node('xycar_driver') self.color_detector = ColorDetector('/usb_cam/image_raw') #self.obstacle_detector = ObstacleDetector('/ultrasonic') #self.color_detector = ColorDectector() self.driver = MotorDriver('/xycar_motor_msg') #self.savedata = [0] #self.save_obs = 130 def trace(self): cir_x, cir_r = self.color_detector.run() if (cir_x != None): if cir_r < 40: angle = 90 + float(50) / 320 * (cir_x - 320) speed = 125 else: angle = 90 speed = 90 else: angle = 90 speed = 90 self.driver.drive(angle, speed) def exit(self): print('finished')
class AutoDrive: def __init__(self): rospy.init_node('xycar_driver') self.line_detector = LineDetector('/usb_cam/image_raw') self.obstacle_detector = ObstacleDetector('/ultrasonic') self.driver = MotorDriver('/xycar_motor_msg') def trace(self): obs_l, obs_m, obs_r = self.obstacle_detector.get_distance() left, right = self.line_detector.direction_info self.line_detector.show_image() angle = self.steer(left, right) speed = self.accelerate(angle, obs_l, obs_m, obs_r) self.driver.drive(angle + 90, speed + 90) def steer(self, left, right): mid = (left + right) // 2 angle = (mid - 320) // 1.7 angle = max(-30, angle) if angle > 0 else min(30, angle) print(angle) return angle def accelerate(self, angle, left, mid, right): # if min(left, mid, right) < 50: # speed = 0 if angle <= -10 or angle >= 10: speed = 22 else: speed = 33 print(speed) return speed def exit(self): print('finished')
class AutoDrive: def __init__(self): rospy.init_node('xycar_driver') self.line_detector = LineDetector('/usb_cam/image_raw') self.driver = MotorDriver('/xycar_motor_msg') def trace(self): left, right = self.line_detector.get_left_right() angle = self.steer(left, right) speed = self.accelerate(angle) self.driver.drive(angle + 90, speed + 90) def steer(self, left, right): print(left, right) mid = (left + right) // 2 - 320 angle = mid // 1.7 angle = max(-50, angle) if angle < 0 else min(50, angle) return angle def accelerate(self, angle): if angle <= -10 or angle >= 10: speed = 20 else: speed = 25 return speed def exit(self): print('finished')
class AutoDrive: def __init__(self): rospy.init_node('xycar_driver') self.line_detector = LineDetector('/usb_cam/image_raw') self.obstacle_detector = ObstacleDetector('/ultrasonic') self.driver = MotorDriver('/xycar_motor_msg') def trace(self): distance = self.obstacle_detector.get_distance() left, right = self.line_detector.direction_info self.line_detector.show_image() angle = self.steer(left, right) speed = self.accelerate(angle, distance) self.driver.drive(angle + 90, speed + 90) def steer(self, left, right): mid = (left + right) // 2 angle = float(mid - 320) / 1.7 print("angle:", angle) return angle def accelerate(self, angle, distance): if distance < 140 and angle >= -14 and angle <= 14 and time.time( ) - start_time >= 50: print("stop!") speed = 0 elif angle <= -12 or angle >= 12: speed = 38 else: speed = 45 print("speed : ", speed) return speed def exit(self): print('finished')
class AutoDrive: def __init__(self): rospy.init_node('xycar_driver') self.line_detector = LineDetector('/usb_cam/image_raw') self.driver = MotorDriver('/xycar_motor_msg') self.obstacledetector = ObstacleDetector('/ultrasonic') def trace(self): left, right = self.line_detector.get_left_right() angle = self.steer(left, right) speed = self.accelerate(angle) self.driver.drive(angle + 90, speed + 90) def steer(self, left, right): mid = (left + right) // 2 - 320 if abs(mid) > 40: angle = mid // 1.5 else: angle = mid // 3.5 angle = max(-50, angle) if angle < 0 else min(50, angle) return angle def accelerate(self, angle): left, mid, right = self.obstacledetector.get_distance() if (left < 140 and mid < 100 and left > 0 and mid > 0): if left + mid < 130: exit() return 0 return 50 def exit(self): print('finished')
class AutoDrive: def __init__(self): rospy.init_node('xycar_driver') self.line_detector = LineDetector('/usb_cam/image_raw') self.driver = MotorDriver('/xycar_motor_msg') self.obstacle_detector = ObstacleDetector('/ultrasonic') self.imu = ImuRead('/diagnostics') def trace(self): obs_l, obs_m, obs_r = self.obstacle_detector.get_distance() line_l, line_r = self.line_detector.detect_lines() self.line_detector.show_images(line_l, line_r) r, p, y = self.imu.get_data() angle = self.steer(line_l, line_r) speed = self.accelerate(angle, obs_l, obs_m, obs_r) self.driver.drive(angle + 90, speed + 90) print(speed + 90) print(r, p, y) def steer(self, left, right): if left == -1: if 320 < right < 390: angle = -50 else: angle = (550 - right) / (-3) elif right == -1: if 250 < left < 320: angle = 50 else: angle = (left - 90) / 3 else: angle = 0 return angle def accelerate(self, angle, left, mid, right): print(mid) if mid < 40: speed = 0 return speed if angle <= -40 or angle >= 40: speed = 30 elif angle <= -25 or angle >= 25: speed = 40 else: speed = 50 return speed def exit(self): print('finished')
class AutoDrive: def __init__(self): rospy.init_node('xycar_driver') self.line_detector = LineDetector('/usb_cam/image_raw') self.driver = MotorDriver('/xycar_motor_msg') def trace(self): line_l, line_r = self.line_detector.detect_lines() self.line_detector.show_images(line_l,s line_r) angle = self.steer(line_l, line_r) speed = self.accelerate(angle) self.driver.drive(angle + 90, speed + 90) def steer(self, left, right): if left == -1: if 320 < right < 400: angle = -58 elif 400 <= right < 480: angle = -40 else: angle = -30 elif right == -1: if 240 <= left < 320: angle = 50 elif 160 <= left < 240: angle = 40 else: angle = 30 else: angle = 0 return angle def accelerate(self, angle): if angle < -20 or angle > 20: speed = 20 else: speed = 30 return speed def exit(self): print('finished')
class AutoDrive: def __init__(self): self.start_time = time.time() self.angle = 0 self.speed = 0 rospy.init_node('xycar_driver') self.line_detector = LineDetector('/usb_cam/image_raw') self.driver = MotorDriver('/xycar_motor_msg') self.obstacle_detector = ObstacleDetector('/ultrasonic') def trace(self): obs_l, obs_m, obs_r = self.obstacle_detector.get_distance() r, l = self.line_detector.detect_lines() self.line_detector.show_images() self.angle = self.steer(l, r, self.angle) self.speed = self.accelerate(self.angle, obs_m) self.driver.drive(self.angle + 90, self.speed + 90) def steer(self, l, r, angle): mid = (l + r) // 2 angle = -((320 - mid) // 1.6) if mid < 320: l = -1 elif mid > 320: r = -1 else: angle = 0 return angle def accelerate(self, angle, l, r): speed = 40 if l == -1 and r == -1: speed = 0 return speed def exit(self): print('finished')
class AutoDrive: def __init__(self): rospy.init_node('xycar_driver') self.detector = Detector('/usb_cam/image_raw') self.driver = MotorDriver('/xycar_motor_msg') self.cnt = 0 self.hipass = False self.tollgateMode = False self.tollbar = False def trace(self): line_l, line_r = self.detector.detect_lines()[0], self.detector.detect_lines()[1] angle, speed = self.steer(line_l, line_r) if self.detector.sign_detect(): self.tollgateMode = True self.tollbar = self.detector.tollbar_detect() if self.tollgateMode: if self.tollbar: self.driver.drive(angle + 90, 90) else: self.driver.drive(angle + 90, 120) # speed + 90 - 60 else: self.driver.drive(angle + 90, speed + 90) print("tollgatemode: ",self.tollgateMode, " tollbar: ",self.tollbar) if self.tollbar is False: self.cnt += 1 if self.tollgateMode and self.cnt > 90: self.tollgateMode = False self.cnt = 0 def steer(self, right, left): maxSpeed = 60 minSpeed = 40 maxAngle = 80 mid = (left + right) // 2 if self.tollgateMode & self.hipass: mid -= 90 elif self.tollgateMode is True and self.hipass is False: mid += 75 if mid < 150: angle = -(max((150 - mid), 1) * maxAngle / 150) speed = max((mid * maxSpeed / 150), minSpeed) elif mid > 150: angle = (max((mid - 150), 1) * maxAngle / 150) speed = max(((300 - mid) * maxSpeed / 150), minSpeed) else: angle = 0 speed = maxSpeed return angle, speed def exit(self): print('finished')
class AutoDrive: def __init__(self): rospy.init_node('xycar_driver') self.line_detector = LineDetector('/usb_cam/image_raw') self.driver = MotorDriver('/xycar_motor_msg') def trace(self): result = self.line_detector.get_left_right() if result: if result == 'left': angle = 30 elif result == 'right': angle = -30 else: left, right = result angle = self.steer(left, right) speed = self.accelerate(angle) else: angle = 0 speed = 0 self.driver.drive(angle + 90, speed + 90) def steer(self, left, right): mid = (left + right) // 2 - 320 if abs(mid) > 40: angle = mid // 1.5 else: angle = mid // 3.5 angle = max(-50, angle) if angle < 0 else min(50, angle) return angle def accelerate(self, angle): return 31 def exit(self): print('finished')
class AutoDrive: def __init__(self): rospy.init_node('xycar_driver') self.line_detector = LineDetector('/usb_cam/image_raw') self.obstacle_detector = ObstacleDetector('/ultrasonic') self.driver = MotorDriver('/xycar_motor_msg') self.speed = 0 self.speed2 = 0 # 장애물, 차선 인식하여 조향각, 속도 결정, 모터 제어 메시지 발생 def trace(self): obs_l, obs_m, obs_r = self.obstacle_detector.get_distance() left, right = self.line_detector.detect_lines() self.len30, self.len40, self.len50 = self.line_detector.img_match() print(self.len30, self.len40, self.len50) point = self.line_detector.show_images(left, right) # 디버깅위한 화면 표시 angle = self.steer(left, right) speed = self.accelerate(angle, obs_l, obs_m, obs_r, point) # 조향각과 속도 인자 결정 self.driver.drive(angle + 90, speed + 90) # 모터 제어를 위한 메서드 호출 # 왼쪽/오른쪽 차선 위치에 따라 조향각 결정 # 가운데 : 0, 오른쪽 : 양수, 왼쪽 : 음수 def steer(self, left, right): angle = 0 mid = (left + right) // 2 angle = int((mid - 320) * 0.5) if angle >= 30: angle = 50 elif angle <= -30: angle = -50 return angle # 조향각, 장애물 거리에 따라 속도 결정 def accelerate(self, angle, left, mid, right, point): if self.len30 > 30: self.speed = 5 print("speed is 30") elif self.len40 > 20: self.speed = 10 print("speed is 40") elif self.len50 > 15: self.speed = 15 print("speed is 50") if min(left, mid, right) < 100: speed = min(left, mid, right) - 60 if speed < 10: speed = 0 elif angle < -20 or angle > 20: speed = 0 else: speed = 0 # 40 if len(point) == 0: pass else: if point[1] >= 155 and point[2] >= 155: pass elif 180 <= point[2]: print("red") self.speed2 = 0 self.speed = 0 elif 150 <= point[1]: print("green") self.speed2 = 15 return self.speed2 + self.speed def exit(self): print('finished')
class AutoDrive: def __init__(self): rospy.init_node('xycar_driver') self.line_detector = LineDetector('/usb_cam/image_raw') self.obstacle_detector = ObstacleDetector('/ultrasonic') self.driver = MotorDriver('/xycar_motor_msg') self.slow_time = time.time() self.prev_dis = 100 self.prev_l = [] self.prev_m = [] self.prev_r = [] self.MAX_SIZE = 3 def average(self, L): if len(L) == 0: return 100 return sum(L) / len(L) def trace(self): obs_l, obs_m, obs_r = self.obstacle_detector.get_distance() line_theta, left, right = self.line_detector.detect_lines() angle = self.steer(line_theta) speed = self.accelerate(line_theta) print(line_theta) print("left: {} mid: {} right: {}".format(obs_l, obs_m, obs_r)) cnt = 0 s = 0 cos_theta = 0.9 dis = self.prev_dis obs_l *= cos_theta obs_r *= cos_theta if obs_l != 0: if len(self.prev_l) + 1 >= self.MAX_SIZE: self.prev_l.pop(0) self.prev_l.append(obs_l) if obs_l <= 70: cnt += 1 s += obs_l if obs_m != 0: if len(self.prev_m) + 1 >= self.MAX_SIZE: self.prev_m.pop(0) self.prev_m.append(obs_m) if obs_m <= 70: cnt += 2 s += obs_m if obs_r != 0: if len(self.prev_r) + 1 >= self.MAX_SIZE: self.prev_r.pop(0) self.prev_r.append(obs_r) if obs_r <= 70: cnt += 1 s += obs_r if cnt >= 2: dis = s / cnt start_time = time.time() if ((cnt >= 3) or (obs_m != 0 and obs_m <= 70 and self.average(self.prev_m) <= 75)) or ( self.average(self.prev_l) <= 75 and self.average(self.prev_m) <= 75 and self.average( self.prev_r) <= 75): if time.time() - start_time > 50: for i in range(2): self.driver.drive(90, 90) time.sleep(0.1) self.driver.drive(90, 60) time.sleep(0.1) self.driver.drive(90, 90) time.sleep(5) else: self.driver.drive(angle + 90 + 5.7, speed) if cnt >= 2: self.prev_dis = dis def steer(self, theta): threshold = 2.0 if -0.8 <= theta <= 0.8: threshold = 8.0 elif -5 <= theta <= 5: threshold = 3.0 elif -10 <= theta <= 10: threshold = 2.3 elif -15 <= theta <= 15: if theta < 0: threshold = 2.0 else: threshold = 2.5 else: if theta < 0: threshold = 2.5 else: threshold = 3.5 angle = theta * threshold return angle def accelerate(self, theta): K = 135 if abs(theta) > 4: self.slow_time = time.time() + 2 if time.time() < self.slow_time: K = 130 speed = K # - min(abs(theta)/2, 10) return speed def exit(self): print('finished')
class AutoDrive: def __init__(self): rospy.init_node('xycar_driver') self.line_detector = LineDetector('/usb_cam/image_raw') self.obstacle_detector = ObstacleDetector('/ultrasonic') self.driver = MotorDriver('/xycar_motor_msg') self.trafficlight = Trafficlight('/usb_cam/image_raw') self.savedata = [0] self.save_obs = 130 def trace(self): obs_m = self.obstacle_detector.get_distance() light = self.trafficlight.found if obs_m / 2 >= 30 and int(obs_m) != 0: speed = int(obs_m / 2) + 60 self.save_obs = speed print(obs_m) else: speed = self.save_obs l_s, r_s = self.line_detector.run() if abs(l_s) >= 0.3 and abs(r_s) >= 0.3: if abs(l_s) > 2.5: angle = -8 self.savedata[0] = angle #print("little left") elif abs(r_s) > 2.5: angle = 8 self.savedata[0] = angle #print("little right") else: angle = 0 self.savedata[0] = angle #print("go") elif (l_s == 0) and (r_s == 0): angle = self.savedata[0] #print("maybe go 111") elif (l_s == 0): angle = -50 self.savedata[0] = angle #print("left") elif (r_s == 0): angle = 50 self.savedata[0] = angle #print("right") else: if abs(l_s) < 0.3: angle = 50 self.savedata[0] = angle #print("maybe right") elif abs(r_s) < 0.3: angle = -50 self.savedata[0] = angle #print("maybe left") else: angle = self.savedata[0] #print("maybe go 222") self.sum = (r_s + l_s) if not light[0]: self.driver.drive(90 + angle, speed) def exit(self): print('finished')
class AutoDrive: def __init__(self): self.start_time = 0 self.cur_time = 0 self.lastAngle = 0 rospy.init_node('xycar_driver') self.line_detector = LineDetector('/usb_cam/image_raw', ) self.obstacle_detector = ObstacleDetector('/ultrasonic') self.driver = MotorDriver('/xycar_motor_msg') self.taxi_driver = TaxiDriver() self.angle_threshold = 10 #minus abs of angle while steering self.angle_is_go = 10 # < 2, go straight self.angle_change_speed = 15 # angle > 5, change speed (lower) self.curve_radius = 10000 #230 self.is_stop = False self.lastObs = [-1, -1, -1] self.obs_list_num = 5 self.obs_l_list = [] self.obs_m_list = [] self.obs_r_list = [] def trace(self): obs_l, obs_m, obs_r = self.obstacle_detector.get_distance() #print(obs_l, obs_m, obs_r) self.lastObs = [obs_l, obs_m, obs_r] angle = self.steer(self.line_detector.detect_angle()) speed = self.accelerate(angle, obs_l, obs_m, obs_r) #print('Angle:', angle, ' Speed:', speed) self.driver.drive(angle + 90, speed + 90) def steer(self, _angle): if _angle == -99: return self.lastAngle #lastangle angle_ret = _angle angle_ret = max(-50, min(angle_ret, 50)) angle_weight = self.curve_radius - math.sqrt(self.curve_radius * self.curve_radius - angle_ret * angle_ret) #print(angle_weight) if angle_ret > 0: angle_ret -= angle_weight else: angle_ret += angle_weight self.lastAngle = angle_ret return angle_ret def accelerate(self, angle, left, mid, right): if len(self.obs_l_list) < 5: if left != 0: self.obs_l_list.append(left) if mid != 0: self.obs_m_list.append(mid) if right != 0: self.obs_r_list.append(right) else: if left != 0: self.obs_l_list = self.obs_l_list[:3] self.obs_l_list.append(left) if mid != 0: self.obs_m_list = self.obs_m_list[:3] self.obs_r_list.append(mid) if right != 0: self.obs_r_list = self.obs_r_list[:3] self.obs_r_list.append(right) #left = sum(self.obs_l_list) / len(self.obs_l_list) #mid = sum(self.obs_m_list) / len(self.obs_m_list) #right = sum(self.obs_r_list) / len(self.obs_r_list) if self.start_time == 0: self.start_time = time.time() self.cur_time = time.time() #print(self.cur_time - self.start_time) if mid < 90 and ((self.cur_time - self.start_time) >= 73.5): self.is_stop = True #print('stop') else: pass if self.is_stop: return 0 if angle < -self.angle_change_speed or angle > self.angle_change_speed: speed = 41 else: speed = 50 if self.taxi_driver.drive == False: return 0 return speed def taxi(self): self.taxi_driver.procFind(self.line_detector.cam_view) self.taxi_driver.procPickup() self.taxi_driver.procDrive() self.taxi_driver.procArrive(self.line_detector.cam_view) self.taxi_driver.procPay() def exit(self): print('finished')
class AutoDrive: def __init__(self): rospy.init_node('xycar_driver') self.line_detector = LineDetector('/usb_cam/image_raw') self.obstacle_detector = ObstacleDetector('/ultrasonic') self.driver = MotorDriver('/xycar_motor_msg') def trace(self): obs_l, obs_m, obs_r = self.obstacle_detector.get_distance() line_l, line_r = self.line_detector.detect_lines() self.line_detector.show_images(line_l, line_r) angle = self.steer(line_l, line_r) speed = self.accelerate(angle, obs_l, obs_m, obs_r) self.driver.drive(angle + 90, speed + 90) def steer(self, left, right): mid = (left + right) // 2 angle = 0 if 600 <= right < 650: if mid > 360: angle -= 20 else: angle += 0 elif 510 <= right < 600: angle -= 30 if mid < 280: angle -= 10 else: angle -= 40 if -10 <= left < 65: if mid < 280: angle += 20 else: angle += 0 elif 65 <= left < 120: angle += 30 if mid > 360: angle += 10 else: angle += 40 print( "reft", left, "mid", mid, "right", right, ) print("") return angle def accelerate(self, angle, left, mid, right): if min(left, mid, right) < 50: speed = 0 if angle < -20 or angle > 20: speed = 20 else: speed = 30 return speed def exit(self): print('finished')
class AutoDrive: def __init__(self): rospy.init_node('xycar_driver') self.line_detector = LineDetector('/usb_cam/image_raw') self.obstacle_detector = ObstacleDetector('/ultrasonic') self.driver = MotorDriver('/xycar_motor_msg') self.left = 0 self.right = 640 self.angle = 0 # 장애물, 차선 인식하여 조향각, 속도 결정, 모터 제어 메시지 발생 def trace(self): if self.line_detector.parkingMatch() == True: self.parking() else: obs_l, obs_m, obs_r = self.obstacle_detector.get_distance() left, right = self.line_detector.detect_lines() self.line_detector.show_images(left, right) # 디버깅위한 화면 표시 angle = self.steer(left, right) speed = self.accelerate(angle, obs_l, obs_m, obs_r) # 조향각과 속도 인자 결정 self.driver.drive(angle + 90, speed + 90) # 모터 제어를 위한 메서드 호출 # 왼쪽/오른쪽 차선 위치에 따라 조향각 결정 # 가운데 : 0, 오른쪽 : 양수, 왼쪽 : 음수 def steer(self, left, right): angle = 0 mid = (left + right) // 2 angle = int((mid - 320) * 0.5) if angle >= 30: angle = 50 elif angle <= -30: angle = -50 return angle # 조향각, 장애물 거리에 따라 속도 결정 & def parking(self): parking_0, parking_3, parking_4, parking_6 = self.obstacle_detector.for_parking_distance( ) while parking_6 <= 50: for go1 in range(5): drive(85, 110) if parking_6 >= 50: time.sleep(1) break time.sleep(0.1) for stop1 in range(2): drive(90, 90) time.sleep(0.1) drive(65, 110) time.sleep(0.1) for left1 in range(50): drive(65, 110) if data_0 <= 5: time.sleep(1) break time.sleep(0.1) for stop2 in range(2): drive(90, 90) time.sleep(0.1) drive(180, 70) time.sleep(0.1) for back1 in range(30): drive(180, 70) if data_3 <= 5: time.sleep(1) break time.sleep(0.1) for stop3 in range(2): drive(90, 90) time.sleep(0.1) drive(70, 110) time.sleep(0.1) for go2 in range(20): drive(70, 110) if data_0 <= 5: time.sleep(1) break time.sleep(0.1) for stop4 in range(2): drive(90, 90) time.sleep(0.1) drive(140, 70) time.sleep(0.1) for back2 in range(5): drive(140, 70) time.sleep(0.1) for stop5 in range(2): drive(90, 90) time.sleep(0.1) drive(90, 110) time.sleep(0.1) for go3 in range(5): drive(90, 110) time.sleep(0.1) for stop6 in range(2): drive(90, 90) time.sleep(0.1) drive(90, 70) time.sleep(0.1) for back3 in range(50): drive(90, 70) if data_4 <= 5: time.sleep(1) break time.sleep(0.1) for finish in range(2): drive(90, 90) time.sleep(0.1) drive(90, 90) time.sleep(0.1) rate.sleep() break rospy.on_shutdown(exit_node) def accelerate(self, angle, left, mid, right): if min(left, mid, right) < 100: speed = min(left, mid, right) - 60 if speed < 10: speed = 0 elif angle < -20 or angle > 20: speed = 0 else: speed = 0 # 4 return speed def exit(self): print('finished')
class AutoDrive: def __init__(self): rospy.init_node('xycar_driver') self.line_detector = LineDetector('/usb_cam/image_raw') self.obstacle_detector = ObstacleDetector('/ultrasonic') self.driver = MotorDriver('/xycar_motor_msg') self.value_filter = MovingAverage(5) self.obs_filter = MovingAverage(5) self.stop_time = None def trace(self): line_l, line_r = self.line_detector.detect_lines() left, mid, right = self.obstacle_detector.get_distance() red, green, yellow = self.line_detector.detect_lights() self.line_detector.show_images(line_l, line_r) angle = self.steer(line_l, line_r) speed = self.accelerate(angle, mid, red, green, yellow) if speed == 0: if not self.stop_time == None && time.time() - self.stop_time >= 10: angle = 45 speed = 40 else: self.stop_time = time.time() else: self.stop_time = None if angle != None: self.driver.drive(angle + 90, speed + 90) def steer(self, left, right): mid = (left + right) // 2 self.value_filter.add_sample(mid) if len(self.value_filter.data) >= 3: if 280 < mid < 360: angle = 0 print(angle) else: angle = int((self.value_filter.get_wmm() - 320) / 1.5) print(angle) return angle def accelerate(self, angle, mid, red, green, yellow): if angle < -50 or angle > 50: speed = 40 else: speed = 40 self.obs_filter.add_sample(mid) if len(self.obs_filter.data) >= 3: if self.obs_filter.get_wmm() <= 50: speed = 0 if red: speed = 0 elif yellow: speed = 25 elif green: speed = 40 return speed def exit(self): print('finished')
class AutoDrive: def __init__(self): rospy.init_node('autodrive') rospy.Subscriber('/signal', String, self.detect_signal) self.line_detector = LineDetector('/usb_cam/image_raw') self.driver = MotorDriver('/xycar_motor_msg') self.end_time = -1 self.signal_time = 3 self.under_signal = "not" def trace(self): line_l, line_r = self.line_detector.get_line() angle = self.steer(line_l, line_r) speed = self.accelerate() self.driver.drive(angle + 90, speed + 90) def steer(self, left, right): mid = (left + right) // 2 angle = (mid - 320) * 0.5 angle = angle / 50 isminus = angle < 0 angle = math.pow(abs(angle), 1.7) if isminus: angle = -angle angle = angle * 50 if angle > 50: angle = 50 elif angle < -50: angle = -50 return angle def accelerate(self): if self.under_signal == "stop": return 0 if self.under_signal != "not": if self.under_signal == "50": return 35 else: return 15 else: return 25 def detect_signal(self, data): if time.time() < self.end_time: return signal = data if signal != "not": self.end_time = time.time() + self.signal_time self.under_signal = signal else: self.under_signal = signal def exit(self): print('finished')
class AutoDrive: def __init__(self): rospy.init_node('xycar_driver') self.line_detector = LineDetector('/usb_cam/image_raw') self.driver = MotorDriver('/xycar_motor_msg') self.obstacle_detector = ObstacleDetector('/ultrasonic') self.imu = ImuRead('/diagnostics') self.information = { "speed": -1, "signal": "", "=>": "", "pitch": -1, "roll": -1, "yaw": -1 } def trace(self): obs_l, obs_m, obs_r = self.obstacle_detector.get_distance() line_l, line_r = self.line_detector.detect_lines() # color_red, color_green, color_blue = self.line_detector.detect_colors() self.line_detector.show_images(line_l, line_r) self.information["roll"], self.information["pitch"], self.information[ "yaw"] = self.imu.get_data() # self.line_detector.show_colors(color_red, color_green, color_blue) angle = self.steer(line_l, line_r) speed = self.accelerate(angle, obs_l, obs_m, obs_r) self.information["speed"] = speed + 90 # print('R (%.1f), P (%.1f), Y (%.1f)' % (self.r, self.p, self.y)) self.driver.drive(angle + 90, speed + 90) # if self.obstacle_stop(obs_l, obs_m, obs_r) == 0: # self.driver.drive(90, 90) # else: # self.driver.drive(angle + 90, speed + 90) def steer(self, left, right): if left == -1: if 320 < right < 390: angle = -50 else: angle = (550 - right) / (-3) elif right == -1: if 250 < left < 320: angle = 50 else: angle = (left - 90) / 3 else: angle = 0 return angle def accelerate(self, angle, left, mid, right): # print(mid) if 0 < mid < 85: speed = 0 return speed if angle <= -40 or angle >= 40: speed = 30 elif angle <= -25 or angle >= 25: speed = 40 else: speed = 50 return speed def printInformation(self): print("speed : " + str(self.information["speed"]) + " ", "signal : " + str(self.information["signal"]) + " ", "=> " + str(self.information["=>"])) print("pitch : " + str(self.information["pitch"]) + " ", "roll : " + str(self.information["roll"]) + " ", "yaw : " + str(self.information["yaw"])) print("") def exit(self): print('finished')
class AutoDrive: def __init__(self): rospy.init_node('xycar_driver') self.line_detector = LineDetector('/usb_cam/image_raw') self.obstacle_detector = ObstacleDetector('/ultrasonic') self.driver = MotorDriver('/xycar_motor_msg') self.slow_time = time.time() self.prev_dis = 100 self.prev_l = [] self.prev_m = [] self.prev_r = [] self.MAX_SIZE = 3 self.isstop = False self.isstart = False def average(self, L): if len(L) == 0: return 100 return sum(L) / len(L) def trace(self): obs_l, obs_m, obs_r = self.obstacle_detector.get_distance() line_theta,left,right = self.line_detector.detect_lines() line_theta += 0.4 angle = self.steer(line_theta,left,right) speed = self.accelerate(angle,line_theta,left,right) print(line_theta) #print(line_theta, left, right) print("left: {} mid: {} right: {}".format(obs_l,obs_m,obs_r)) cnt = 0 s = 0 cos_theta = 0.9 dis = self.prev_dis obs_l *= cos_theta obs_r *= cos_theta if obs_l != 0: if len(self.prev_l) + 1 >= self.MAX_SIZE: self.prev_l.pop(0) self.prev_l.append(obs_l) if obs_l <= 90: cnt +=1 s += obs_l if obs_m != 0: if len(self.prev_m) + 1 >= self.MAX_SIZE: self.prev_m.pop(0) self.prev_m.append(obs_m) if obs_m <= 90: cnt +=2 s += obs_m if obs_r != 0: if len(self.prev_r) + 1 >= self.MAX_SIZE: self.prev_r.pop(0) self.prev_r.append(obs_r) if obs_r <=90: cnt +=1 s += obs_r if cnt >=2: dis = s/cnt if -1 <= line_theta <= 1 and cnt >=3 and self.isstart: if self.isstop: self.driver.drive(90,90) else: self.isstop = True for i in range(2): self.driver.drive(90,90) time.sleep(0.1) self.driver.drive(90,50) time.sleep(0.1) self.driver.drive(90,90) #time.sleep(3) rospy.signal_shutdown('Quit') else: self.isstop = False self.driver.drive(angle + 90+ 2.77, speed) if cnt >=2: self.prev_dis = dis if line_theta > 10: self.isstart = True def steer(self, theta, left, right): """ weight = 0.0 if left == -1: weight = 0.0 elif right == -1: weight = 0.0 else: mid = (left + right) / 2 diff = 55-mid if abs(diff) < 3: weight = 0.0 # car is at right elif diff < 0: weight = -1.0 elif diff > 0: weight = 1.0 """ K = 0.0 if -0.8 <= theta <= 0.8: K = 8.0 elif -5 <= theta <= 5: K = 2.0 elif -10 <= theta <= 10: K = 2.3 elif -15 <= theta <= 15: ''' if theta < 0: if theta > -10: K = 1.86 else: K = 1.4 else: K = 1.75 ''' if theta < 0: K = 2.0 else: K = 2.0 else: #elif abs(theta) < 30: if theta < 0: K = 3.0 else: K = 3.0 #else: # K = ''' elif theta >= 20: K = 3.0 elif theta <= -20: K = 2.1 else: K = 1.5 ''' """ if theta > 0: K = 1.75 else: K = 1.6 """ angle = theta * K return angle #+ (weight * 15) #return angle def accelerate(self, angle, theta, left, right): K = 140 """ if abs(theta) > 4: self.slow_time = time.time() + 2 if time.time() < self.slow_time: K = 130 """ speed = K# - min(abs(theta)/2, 10) return speed def exit(self): print('finished')
class AutoDrive: def __init__(self): rospy.init_node('xycar_driver') self.line_detector = LineDetector('/usb_cam/image_raw') self.obstacle_detector = ObstacleDetector('/ultrasonic') self.driver = MotorDriver('/xycar_motor_msg') self.imu = ImuRead('/diagnostics') self.b_l = 0 self.b_r = 0 self.origin = False def trace(self): obs_l, obs_m, obs_r = self.obstacle_detector.get_distance() line_l, line_r = self.line_detector.detect_lines() self.line_detector.show_images(line_l, line_r) r, p = self.imu.get_data() angle = self.steer(line_l, line_r) speed = self.accelerate(angle, obs_l, obs_m, obs_r, r, p) if self.origin and p > -6.5: speed = -7 angle = 0 elif ((r > 4 and r < 175.0) or (r < -4 and r > -175.0)) and self.origin == False: speed = 28 # 30 angle = -5 elif p <= -6.5 and p > -7.5: speed = -1 angle = 0 elif p <= -7.5 and p > -8.5: speed = 24 angle = 0 elif p <= -8.5 and p >= -15: speed = 26 # 28 angle = 0 elif p < -15: speed = 34.5 angle = 0 self.origin = True ''' elif (r>0 and r<175.0) or (r<0 and r>-175.0): speed = 30 angle = -5 elif line_l == -2: speed = 0 angle = 0 print("it stopped") ''' print("angle-speed:", angle, speed) print("roll:", r, p) # print('R (%.1f) P (%.1f), Y (%.1f)' % (r, p, y)) self.driver.drive(angle + 90, speed + 90) def steer(self, left, right): if left == -1 and right == 641: left = self.b_l right = self.b_r mid = (left + right) // 2 if mid < 250: angle = -82 elif mid >= 250 and mid < 260: angle = -70 elif mid >= 260 and mid < 270: angle = -40 # angle - -55 elif mid >= 270 and mid < 280: # angle = -50 angle = -30 elif mid >= 280 and mid < 285: angle = -10 elif mid > 390: angle = 73 elif mid >= 380 and mid < 390: angle = 50 elif mid >= 370 and mid < 380: angle = 38 elif mid >= 360 and mid < 370: angle = 28 elif mid >= 355 and mid < 360: angle = 1 else: angle = 0 self.b_l = left self.b_r = right print(self.b_l, self.b_r) print(left, right) return angle def accelerate(self, angle, left, mid, right, roll, pitch): after = time.time() print(mid, after - now) # if mid < 90: #and (after-now) > 70.5: # speed = 20 # if mid < 85: #and (after-now) > 70.5: # speed = 12 # if mid < 80: #and (after-now) > 70.5: # speed = 7 # if mid < 85: #and (after-now) > 70.5: # speed = 4 if mid < 95 and (after - now) < 20.5: speed = 0 elif angle == 10 or angle == -10: speed = 32 elif angle < -30 or angle > 30: speed = 34 else: speed = 35 return speed def exit(self): print('finished')
class AutoDrive: def __init__(self): rospy.init_node('xycar_driver') self.line_detector = LineDetector('/usb_cam/image_raw') self.traffic_light_detector = TrafficLightDetector('/usb_cam/image_raw') self.vehicle_breaker_detector = VehicleBreakerDetector('/usb_cam/image_raw') self.bus_stop_detector = BusStopDetector('/usb_cam/image_raw') self.obstacle_detector = ObstacleDetector('/ultrasonic') self.driver = MotorDriver('/xycar_motor_msg') self.slow_time = time.time() self.prev_dis = 100 self.prev_l = [] self.prev_m = [] self.prev_r = [] self.MAX_SIZE = 3 self.bus_stop_time = time.time() self.bus_ignore_time = -1 self.bus_data = [] def average(self, L): if len(L) == 0: return 100 return sum(L) / len(L) def trace(self): k = 500 if len(self.bus_data) > 4: self.bus_data.pop(0) self.bus_data.append((self.bus_stop_detector.detect())) bus_stop, stop_m, stop_M, people_cnt = 0, [0, 0], [0, 0], 0 #self.bus_data[-1] for data in self.bus_data: bus_stop += 1 if data[0] else 0 stop_m[0] += data[1][0][0] stop_m[1] += data[1][0][1] stop_M[0] += data[1][1][0] stop_M[1] += data[1][1][1] people_cnt += data[2] bus_stop = bus_stop > len(self.bus_data) // 2 stop_m[0] //= len(self.bus_data) stop_m[1] //= len(self.bus_data) stop_M[0] //= len(self.bus_data) stop_M[1] //= len(self.bus_data) people_cnt /= len(self.bus_data) ''' if bus_stop and (stop_M[0] - stop_m[0]) * (stop_M[1] - stop_m[1]) > (208 - 164) * (100 - 56): time.sleep(3.0) self.driver.drive(90, 90) time.sleep(people_cnt * 2.0) ''' if bus_stop and (stop_M[0] - stop_m[0]) * (stop_M[1] - stop_m[1]) > (208 - 164) * (100 - 56): if self.bus_stop_time < time.time(): #time.sleep(2.0) #self.bus_stop_time = time.time() + 2.0 pass if people_cnt > 0 and self.bus_ignore_time < time.time(): self.bus_stop_time = time.time() + 2.0 elif self.bus_stop_time > time.time(): pass #self.bus_ignore_time = time.time() + 5.0 print(bus_stop, stop_m, stop_M, people_cnt) if self.bus_stop_time > time.time() and self.bus_ignore_time < time.time(): self.driver.drive(90, 90) else: self.driver.drive(90, 115) return breaker_ret, cnt = self.vehicle_breaker_detector.detect() print(cnt) if breaker_ret: self.driver.drive(90,90) else: self.driver.drive(90,115) return light_color = self.traffic_light_detector.detect() print(light_color) if light_color == 'green': self.driver.drive(90, 115) elif light_color == 'orange': self.driver.drive(90, 110) elif light_color == 'red': self.driver.drive(90, 90) return obs_l, obs_m, obs_r = self.obstacle_detector.get_distance() line_theta,left,right = self.line_detector.detect_lines() line_theta += 0.4 angle = self.steer(line_theta,left,right) speed = self.accelerate(angle,line_theta,left,right) print(line_theta) #print(line_theta, left, right) print("left: {} mid: {} right: {}".format(obs_l,obs_m,obs_r)) cnt = 0 s = 0 cos_theta = 0.9 dis = self.prev_dis obs_l *= cos_theta obs_r *= cos_theta if obs_l != 0: if len(self.prev_l) + 1 >= self.MAX_SIZE: self.prev_l.pop(0) self.prev_l.append(obs_l) if obs_l <= 70: cnt +=1 s += obs_l if obs_m != 0: if len(self.prev_m) + 1 >= self.MAX_SIZE: self.prev_m.pop(0) self.prev_m.append(obs_m) if obs_m <= 70: cnt +=2 s += obs_m if obs_r != 0: if len(self.prev_r) + 1 >= self.MAX_SIZE: self.prev_r.pop(0) self.prev_r.append(obs_r) if obs_r <=70: cnt +=1 s += obs_r if cnt >=2: dis = s/cnt if ((cnt >=3) or (obs_m != 0 and obs_m <=70 and self.average(self.prev_m) <= 75)) or (self.average(self.prev_l) <= 75 and self.average(self.prev_m) <= 75 and self.average(self.prev_r) <=75): for i in range(2): self.driver.drive(90,90) time.sleep(0.1) self.driver.drive(90,60) time.sleep(0.1) self.driver.drive(90,90) time.sleep(5) else: self.driver.drive(angle + 90+ 2.77, speed) if cnt >=2: self.prev_dis = dis def steer(self, theta, left, right): """ weight = 0.0 if left == -1: weight = 0.0 elif right == -1: weight = 0.0 else: mid = (left + right) / 2 diff = 55-mid if abs(diff) < 3: weight = 0.0 # car is at right elif diff < 0: weight = -1.0 elif diff > 0: weight = 1.0 """ K = 0.0 if -0.8 <= theta <= 0.8: K = 8.0 elif -5 <= theta <= 5: K = 2.0 elif -10 <= theta <= 10: K = 2.3 elif -15 <= theta <= 15: ''' if theta < 0: if theta > -10: K = 1.86 else: K = 1.4 else: K = 1.75 ''' if theta < 0: K = 1.5 else: K = 2.0 else: #elif abs(theta) < 30: if theta < 0: K = 3.0 else: K = 3.0 #else: # K = ''' elif theta >= 20: K = 3.0 elif theta <= -20: K = 2.1 else: K = 1.5 ''' """ if theta > 0: K = 1.75 else: K = 1.6 """ angle = theta * K return angle #+ (weight * 15) #return angle def accelerate(self, angle, theta, left, right): K = 135 if abs(theta) > 4: self.slow_time = time.time() + 2 if time.time() < self.slow_time: K = 130 speed = K# - min(abs(theta)/2, 10) return speed def exit(self): print('finished')
class AutoDrive: def __init__(self): rospy.init_node('xycar_driver') self.line_detector = LineDetector('/usb_cam/image_raw') self.obstacle_detector = ObstacleDetector('/ultrasonic') self.driver = MotorDriver('/xycar_motor_msg') self.imu = ImuRead('diagnostics') self.before = [0, 0, 0] self.before_sonic = 0 self.startTime = time.time() def trace(self): obs_l, obs_m, obs_r = self.obstacle_detector.get_distance() line_l, line_r, line_s = self.line_detector.detect_lines() self.line_detector.show_images(line_l, line_r) r, p, y = self.imu.get_data() angle = self.steer(line_l, line_r, r, p, y) speed = self.accelerate(angle, obs_l, obs_m, obs_r, r, p, y, line_s) self.driver.drive(angle + 90, speed + 90) # print(r, p, y, "imu") #print(self.read) def steer(self, left, right, r, p, y): mid = (left + right) // 2 if left == -1 and right == -1: left, right, mid = self.before[0], self.before[1], self.before[2] if left > 0 and right > 0 and mid < 150: left, right, mid = self.before[0], self.before[1], self.before[2] # if right != -1 and (self.before[2] - mid) > 5: # mid = (mid + self.before[2]) * 1 // 2 + 5 # print((mid + self.before[2]) * 1 // 2 + 5) if left == -1: if mid - 300 < -50: angle = -50 else: angle = mid - 300 elif right == -1: if mid - 15 > 50: angle = 50 else: angle = mid - 15 elif mid > 325: if mid - 325 > 50: angle = 50 else: angle = mid - 325 elif mid < 295: if mid - 295 < -50: angle = -50 else: angle = mid - 295 else: angle = 0 if p > 5: angle = 0 elif p < -5: angle = 0 self.before[0], self.before[1], self.before[2] = left, right, mid return angle def accelerate(self, angle, left, mid, right, r, p, y, stop): if angle > 0: angle = -angle #if mid < 110: # speed = 0 if angle < -30 or angle > 30: speed = 50 elif angle < -10: speed = 50 else: speed = 50 if p > 7: speed = -40 elif p < -7: speed = 35 print(left, mid, right, self.before_sonic - mid) self.before_sonic = mid if stop == 1: speed = 0 return speed def exit(self): print('finished')
class AutoDrive: def __init__(self): self.image_width = 640 # 이미지의 너비 rospy.init_node('xycar_driver') self.driver = MotorDriver('/xycar_motor_msg') self.cam_image = CamImage('/usb_cam/image_raw') self.pub = rospy.Publisher('/remote_rec', Image, queue_size=1) rospy.Subscriber('/remote_pub', Int32MultiArray, self.get_yolo_result) rospy.Subscriber('/xycar_ad_controller/controller_msg', Int32MultiArray, self.get_controller_msg) self.cont_pub = rospy.Publisher('/xycar_ad/controller_msg', Int32MultiArray, queue_size=1) self.yolo_dat = None self.x = 50 self.y = 50 self.dir = 0 self.dx = self.x self.dy = self.y self.shortest_path = SPF(100, 100) def get_controller_msg(self, loc): self.dx = loc.data[0] self.dy = loc.data[1] def go_forward(self): self.driver.drive(90, 90 + 30) time.sleep(2) self.driver.drive(90, 90) def get_shortest_path(self): return self.shortest_path.get_path(self.x, self.y, self.dx, self.dy) def add_obstacle(self, x, y): self.shortest_path.set_obs(x, y) def get_yolo_result(self, data): print('recieved') self.yolo_dat = data.data if self.yolo_dat[0] == 1: print('detected!', self.yolo_dat[1:]) else: print('not detected') #print(data.data) def publish_to_remote(self): #pub_info = [1, 1] #pub_info = Int32MultiArray(data=pub_info) dat = self.cam_image.getData() self.pub.publish(dat) def for_bck(self): for stop_cnt in range(2): self.driver.drive(90, 90) time.sleep(0.1) self.driver.drive(90, 70) time.sleep(0.1) def turn(self, ccw=False): angle = 50 if ccw: angle *= -1 self.driver.drive(90, 90 + 30) time.sleep(0.3) for i in range(5): self.turn_(angle) time.sleep(0.5) #self.bwd_stop() self.driver.drive(90, 90 - 20) print('back') time.sleep(0.5) self.bwd_stop() self.driver.drive(90, 90) def turn_(self, angle): self.driver.drive(90 + angle, 90 + 30) time.sleep(0.5) self.fwd_stop() time.sleep(0.5) self.for_bck() self.driver.drive(90 - angle, 90 - 25) time.sleep(0.5) self.bwd_stop() time.sleep(0.5) def fwd_stop(self): self.driver.drive(90, 40) time.sleep(0.1) self.driver.drive(90, 90) def bwd_stop(self): self.driver.drive(90, 140) time.sleep(0.1) self.driver.drive(90, 90)
class AutoDrive: def __init__(self): rospy.init_node('xycar_driver') self.line_detector = LineDetector('/usb_cam/image_raw') self.driver = MotorDriver('/xycar_motor_msg') self.imu = ImuRead('/diagnostics') self.information = {"speed": -1, "signal": "", "=>": "", "pitch": -1, "roll": -1, "yaw": -1} def trace(self): line_l, line_r, red, yellow, green = self.line_detector.detect_lines() self.r, self.y, self.g = red, yellow, green self.line_detector.show_images(line_l, line_r) self.information["roll"], self.information["pitch"], self.information["yaw"] = self.imu.get_data() angle = self.steer(line_l, line_r) speed = self.accelerate(angle, red, yellow, green) self.information["speed"] = speed + 90 self.driver.drive(angle + 90, speed + 90) print(speed + 90) print(r, p, y) def steer(self, left, right): if left == -1: if 320 < right < 390: angle = -50 else: angle = (550 - right) / (-3) elif right == -1: if 250 < left < 320: angle = 50 else: angle = (left - 90) / 3 else: angle = 0 return angle def accelerate(self, angle, red, yellow, green): # if red is detected, stop if red and (not yellow) and (not green): speed = 0 # if yellow is detected, go low speed elif (not red) and yellow and (not green): speed = 20 # if green is detected, go origin speed elif (not red) and (not yellow) and green: speed = 50 # This is origin code else: # (not red) and (not yellow) and (not green): if angle <= -40 or angle >= 40: speed = 30 elif angle <= -25 or angle >= 25: speed = 40 else: speed = 50 print(red, yellow, green) return speed def printInformation(self): if self.r: self.information["signal"] = "red" self.information["=>"] = "stop!" if self.y: self.information["signal"] = "yellow" self.information["=>"] = "slow" if self.g: self.information["signal"] = "green" self.information["=>"] = "just go" print("speed : " + str(self.information["speed"]) + " ", "signal : " + str(self.information["signal"]) + " ", "=> " + str(self.information["=>"])) print("pitch : " + str(self.information["pitch"]) + " ", "roll : " + str(self.information["roll"]) + " ", "yaw : " + str(self.information["yaw"])) print("") self.r, self.y, self.g = False, False, False def exit(self): print('finished')
class AutoDrive: def __init__(self): rospy.init_node('xycar_driver') self.line_detector = LineDetector('/usb_cam/image_raw') self.obstacle_detector = ObstacleDetector('/ultrasonic') self.driver = MotorDriver('/xycar_motor_msg') self.imu = ImuRead('/diagnostics') self.angle = 0 self.speed = 0 def trace(self): redSignal = self.line_detector.trafficLight() print(redSignal) if redSignal: print(redSignal) self.forward() r, p, y = self.imu.get_data() obs_l, obs_m, obs_r, obs_sl, obs_sr, B = self.obstacle_detector.get_distance( ) line_l, line_r = self.line_detector.detect_lines() #self.line_detector.show_images(line_l,line_r) self.angle = self.steer(line_l, line_r) speed = self.accelerate(self.angle, obs_l, obs_m, obs_r, p, obs_sl, obs_sr, B) self.driver.drive(self.angle + 90, speed + 90) #print(p) if (p < -5): time.sleep(3) def steer(self, left, right): if self.angle < 0 and (100 <= left <= 150): return self.angle self.angle = 0 if left == -1: self.angle = -55 elif left < 17: self.angle = -48 elif 17 <= left < 50: self.angle = -19 elif left > 130: self.angle = 47 elif 130 >= left > 100: self.angle = 18 #print(left) return self.angle def accelerate(self, angle, left, mid, right, p, side_left, side_right, B): if mid < 40 and side_left < 40 and side_right < 40: self.backward() if p < -5: return 0 if self.speed > 0 and mid == 0: return self.speed elif (angle <= -25 or angle >= 35): self.speed = 37 else: self.speed = 46 #print(mid) return 25 def exit(self): print('finished') def backward(self): for i in range(2): self.driver.drive(90, 90) time.sleep(0.1) self.driver.drive(90, 60) time.sleep(0.1) for i in range(50): self.driver.drive(35, 70) time.sleep(0.1) def forward(self): for i in range(30): self.driver.drive(35, 120) time.sleep(0.1)