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