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