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')
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 = []
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')
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 __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 __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 __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"
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') 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): 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') if __name__ == '__main__': car = AutoDrive() time.sleep(3) rate = rospy.Rate(10) while not rospy.is_shutdown(): car.trace() rate.sleep() rospy.on_shutdown(car.exit)
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 __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 __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
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')
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 }
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('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') 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') 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')
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 = []
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')
def filter(self, image): # parameters ThumbnailOptions = namedtuple("MyStruct", "resizeX thinningIteration segmentSize \ variation margin minLength lineConnected lineThr lineSurrThr removingMargin \ logo_minDensity logo_minX logo_minY text_wordDistance text_minSizeX text_minSizeY \ thumbnail_X") options = ThumbnailOptions(resizeX = 1000, thinningIteration = 3, segmentSize = 15, \ variation = 2, margin = 5, minLength = 60, lineConnected = 4, lineThr = 0.75, \ lineSurrThr = 0.02, removingMargin = 5, logo_minDensity = 0.1, logo_minX = 60, logo_minY = 60, \ text_wordDistance = 60, text_minSizeX = 10, text_minSizeY = 15, \ thumbnail_X = self.thumbnailwidth) thresholdfilter = ThresholdFilter(options.resizeX) thresholdedImage = thresholdfilter.filter(image) thinningfilter = ThinningFilter(options.thinningIteration) thinnedImage = thinningfilter.filter(thresholdedImage.astype(int)) > 0 thinnedImage = thinnedImage.astype(int) objectremoverfilter = ObjectRemoverFilter(0, options.removingMargin) linedetector = LineDetector(options.segmentSize, options.variation, options.lineThr, options.lineSurrThr, options.lineConnected, options.margin, options.minLength) lines = linedetector.detect(thinnedImage) objectremoverfilter.setobjects(lines) linesRemoved = objectremoverfilter.filter(thresholdedImage.copy()) logodetector = LogoDetector(options.logo_minDensity, options.logo_minX, options.logo_minY) logos = logodetector.detect(linesRemoved) objectremoverfilter.setobjects(logos) logoRemoved = objectremoverfilter.filter(linesRemoved.copy()) textdetector = TextDetector(options.text_wordDistance, options.text_minSizeX, options.text_minSizeY) text = textdetector.detect(logoRemoved) thumbnail_Y = int(options.thumbnail_X/float(len(thresholdedImage))*len(thresholdedImage[0])) thumbnail = self._createThumbnail([len(thresholdedImage), len(thresholdedImage[0])], lines, logos, text, [options.thumbnail_X, thumbnail_Y]) return thumbnail
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): 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')
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')
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): 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') 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')
import time import cv2 from linedetector import LineDetector cap = cv2.VideoCapture('2.avi') detector = LineDetector('', ros_node=False) while True: ret, frame = cap.read() if not ret: break if cv2.waitKey(1) & 0xff == 27: break detector.conv_image(frame) l, r = detector.detect_lines() detector.show_images(l, r) time.sleep(0.03) cap.release() cv2.destroyAllWindows()
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') 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('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')