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