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 __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):
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')
def __init__(self):
MotorDriver.__init__(self)
self.L1 = 0.17996
self.L2 = 0.21085
self.WORKSPACE = 0.39081
self.JOINT_RANGE_MAX = 170
self.JOINT_RANGE_MIN = 10
self.Q_BASE_ZERO_POS_ADJ = 45 * CONST_D2R
self.Q_ELBOW_ZERO_POS_ADJ = self.JOINT_RANGE_MAX * CONST_D2R
self.POS1 = [-0.160, 0.224]
self.POS2 = [-0.110, 0.236]
self.POS3 = [-0.060, 0.245]
self.POS4 = [-0.025, 0.253]
self.POS5 = [-0.0005, 0.258]
self.POS6 = [0.030, 0.263]
self.POS7 = [0.060, 0.266]
self.curPos = [0.006312345009324774, 0.04546727240325704]
self.HOME_POS = [0.006312345009324774, 0.04546727240325704]
def __init__(self):
self.image_width = 640 # 이미지의 너비
rospy.init_node('xycar_driver')
self.driver = MotorDriver('/xycar_motor_msg')
self.cam_image = CamImage('/usb_cam/image_raw')
self.pub = rospy.Publisher('/remote_rec', Image, queue_size=1)
rospy.Subscriber('/remote_pub', Int32MultiArray, self.get_yolo_result)
rospy.Subscriber('/xycar_ad_controller/controller_msg', Int32MultiArray, self.get_controller_msg)
self.cont_pub = rospy.Publisher('/xycar_ad/controller_msg', Int32MultiArray, queue_size=1)
self.yolo_dat = None
self.x = 50
self.y = 50
self.dir = 0
self.dx = self.x
self.dy = self.y
self.shortest_path = SPF(100, 100)
class AutoDrive:
def __init__(self):
self.start_time = time.time()
self.angle = 0
self.speed = 0
rospy.init_node('xycar_driver')
self.line_detector = LineDetector('/usb_cam/image_raw')
self.driver = MotorDriver('/xycar_motor_msg')
self.obstacle_detector = ObstacleDetector('/ultrasonic')
def trace(self):
obs_l, obs_m, obs_r = self.obstacle_detector.get_distance()
r, l = self.line_detector.detect_lines()
self.line_detector.show_images()
self.angle = self.steer(l, r, self.angle)
self.speed = self.accelerate(self.angle, obs_m)
self.driver.drive(self.angle + 90, self.speed + 90)
def steer(self, l, r, angle):
mid = (l + r) // 2
angle = -((320 - mid) // 1.6)
if mid < 320:
l = -1
elif mid > 320:
r = -1
else:
angle = 0
return angle
def accelerate(self, angle, l, r):
speed = 40
if l == -1 and r == -1:
speed = 0
return speed
def exit(self):
print('finished')
class AutoDrive:
def __init__(self):
rospy.init_node('xycar_driver')
self.detector = Detector('/usb_cam/image_raw')
self.driver = MotorDriver('/xycar_motor_msg')
self.cnt = 0
self.hipass = False
self.tollgateMode = False
self.tollbar = False
def trace(self):
line_l, line_r = self.detector.detect_lines()[0], self.detector.detect_lines()[1]
angle, speed = self.steer(line_l, line_r)
if self.detector.sign_detect():
self.tollgateMode = True
self.tollbar = self.detector.tollbar_detect()
if self.tollgateMode:
if self.tollbar:
self.driver.drive(angle + 90, 90)
else:
self.driver.drive(angle + 90, 120) # speed + 90 - 60
else:
self.driver.drive(angle + 90, speed + 90)
print("tollgatemode: ",self.tollgateMode, " tollbar: ",self.tollbar)
if self.tollbar is False:
self.cnt += 1
if self.tollgateMode and self.cnt > 90:
self.tollgateMode = False
self.cnt = 0
def steer(self, right, left):
maxSpeed = 60
minSpeed = 40
maxAngle = 80
mid = (left + right) // 2
if self.tollgateMode & self.hipass:
mid -= 90
elif self.tollgateMode is True and self.hipass is False:
mid += 75
if mid < 150:
angle = -(max((150 - mid), 1) * maxAngle / 150)
speed = max((mid * maxSpeed / 150), minSpeed)
elif mid > 150:
angle = (max((mid - 150), 1) * maxAngle / 150)
speed = max(((300 - mid) * maxSpeed / 150), minSpeed)
else:
angle = 0
speed = maxSpeed
return angle, speed
def exit(self):
print('finished')
class AutoDrive:
def __init__(self):
rospy.init_node('xycar_driver')
self.line_detector = LineDetector('/usb_cam/image_raw')
self.driver = MotorDriver('/xycar_motor_msg')
def trace(self):
result = self.line_detector.get_left_right()
if result:
if result == 'left':
angle = 30
elif result == 'right':
angle = -30
else:
left, right = result
angle = self.steer(left, right)
speed = self.accelerate(angle)
else:
angle = 0
speed = 0
self.driver.drive(angle + 90, speed + 90)
def steer(self, left, right):
mid = (left + right) // 2 - 320
if abs(mid) > 40:
angle = mid // 1.5
else:
angle = mid // 3.5
angle = max(-50, angle) if angle < 0 else min(50, angle)
return angle
def accelerate(self, angle):
return 31
def exit(self):
print('finished')
class AutoDrive:
def __init__(self):
rospy.init_node('autodrive')
rospy.Subscriber('/signal', String, self.detect_signal)
self.line_detector = LineDetector('/usb_cam/image_raw')
self.driver = MotorDriver('/xycar_motor_msg')
self.end_time = -1
self.signal_time = 3
self.under_signal = "not"
def trace(self):
line_l, line_r = self.line_detector.get_line()
angle = self.steer(line_l, line_r)
speed = self.accelerate()
self.driver.drive(angle + 90, speed + 90)
def steer(self, left, right):
mid = (left + right) // 2
angle = (mid - 320) * 0.5
angle = angle / 50
isminus = angle < 0
angle = math.pow(abs(angle), 1.7)
if isminus:
angle = -angle
angle = angle * 50
if angle > 50:
angle = 50
elif angle < -50:
angle = -50
return angle
def accelerate(self):
if self.under_signal == "stop":
return 0
if self.under_signal != "not":
if self.under_signal == "50":
return 35
else:
return 15
else:
return 25
def detect_signal(self, data):
if time.time() < self.end_time:
return
signal = data
if signal != "not":
self.end_time = time.time() + self.signal_time
self.under_signal = signal
else:
self.under_signal = signal
def exit(self):
print('finished')
class AutoDrive:
def __init__(self):
rospy.init_node('xycar_driver')
self.line_detector = LineDetector('/usb_cam/image_raw')
self.obstacle_detector = ObstacleDetector('/ultrasonic')
self.driver = MotorDriver('/xycar_motor_msg')
self.left = 0
self.right = 640
self.angle = 0
# 장애물, 차선 인식하여 조향각, 속도 결정, 모터 제어 메시지 발생
def trace(self):
if self.line_detector.parkingMatch() == True:
self.parking()
else:
obs_l, obs_m, obs_r = self.obstacle_detector.get_distance()
left, right = self.line_detector.detect_lines()
self.line_detector.show_images(left, right) # 디버깅위한 화면 표시
angle = self.steer(left, right)
speed = self.accelerate(angle, obs_l, obs_m,
obs_r) # 조향각과 속도 인자 결정
self.driver.drive(angle + 90, speed + 90) # 모터 제어를 위한 메서드 호출
# 왼쪽/오른쪽 차선 위치에 따라 조향각 결정
# 가운데 : 0, 오른쪽 : 양수, 왼쪽 : 음수
def steer(self, left, right):
angle = 0
mid = (left + right) // 2
angle = int((mid - 320) * 0.5)
if angle >= 30:
angle = 50
elif angle <= -30:
angle = -50
return angle
# 조향각, 장애물 거리에 따라 속도 결정 &
def parking(self):
parking_0, parking_3, parking_4, parking_6 = self.obstacle_detector.for_parking_distance(
)
while parking_6 <= 50:
for go1 in range(5):
drive(85, 110)
if parking_6 >= 50:
time.sleep(1)
break
time.sleep(0.1)
for stop1 in range(2):
drive(90, 90)
time.sleep(0.1)
drive(65, 110)
time.sleep(0.1)
for left1 in range(50):
drive(65, 110)
if data_0 <= 5:
time.sleep(1)
break
time.sleep(0.1)
for stop2 in range(2):
drive(90, 90)
time.sleep(0.1)
drive(180, 70)
time.sleep(0.1)
for back1 in range(30):
drive(180, 70)
if data_3 <= 5:
time.sleep(1)
break
time.sleep(0.1)
for stop3 in range(2):
drive(90, 90)
time.sleep(0.1)
drive(70, 110)
time.sleep(0.1)
for go2 in range(20):
drive(70, 110)
if data_0 <= 5:
time.sleep(1)
break
time.sleep(0.1)
for stop4 in range(2):
drive(90, 90)
time.sleep(0.1)
drive(140, 70)
time.sleep(0.1)
for back2 in range(5):
drive(140, 70)
time.sleep(0.1)
for stop5 in range(2):
drive(90, 90)
time.sleep(0.1)
drive(90, 110)
time.sleep(0.1)
for go3 in range(5):
drive(90, 110)
time.sleep(0.1)
for stop6 in range(2):
drive(90, 90)
time.sleep(0.1)
drive(90, 70)
time.sleep(0.1)
for back3 in range(50):
drive(90, 70)
if data_4 <= 5:
time.sleep(1)
break
time.sleep(0.1)
for finish in range(2):
drive(90, 90)
time.sleep(0.1)
drive(90, 90)
time.sleep(0.1)
rate.sleep()
break
rospy.on_shutdown(exit_node)
def accelerate(self, angle, left, mid, right):
if min(left, mid, right) < 100:
speed = min(left, mid, right) - 60
if speed < 10:
speed = 0
elif angle < -20 or angle > 20:
speed = 0
else:
speed = 0 # 4
return speed
def exit(self):
print('finished')
class AutoDrive:
def __init__(self):
rospy.init_node('xycar_driver')
self.line_detector = LineDetector('/usb_cam/image_raw')
self.obstacle_detector = ObstacleDetector('/ultrasonic')
self.driver = MotorDriver('/xycar_motor_msg')
self.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.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')
def __del__(self):
MotorDriver.__del__(self)
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')
def setPose(self, x, y):
MotorDriver.resetWristHeight(self)
qBase, qElbow, qWrist = self.sLineTraj(x, y)
for i in range(len(qBase)):
MotorDriver.setBaseAngle(self, qBase[i])
MotorDriver.setElbowAngle(self, qElbow[i])
MotorDriver.setWristAngle(self, qWrist[i])
if (x < -0.01):
MotorDriver.setWristHeight(self, WRIST_TO_BOARD_HEIGHT + 15)
else:
MotorDriver.setWristHeight(self, WRIST_TO_BOARD_HEIGHT)
# input()
MotorDriver.eeDropDisk(self)
MotorDriver.resetWristHeight(self)
if ((self.curPos[0] != self.HOME_POS[0])
and (self.curPos[1] != self.HOME_POS[1])):
qBase, qElbow, qWrist = self.sLineTraj(self.HOME_POS[0],
self.HOME_POS[1])
for i in range(len(qBase)):
MotorDriver.setBaseAngle(self, qBase[i])
MotorDriver.setElbowAngle(self, qElbow[i])
MotorDriver.setWristAngle(self, qWrist[i])
time.sleep(0.2)
return qBase, qElbow, qWrist
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.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.driver = MotorDriver('/xycar_motor_msg')
self.imu = ImuRead('/diagnostics')
self.information = {"speed": -1, "signal": "", "=>": "", "pitch": -1, "roll": -1, "yaw": -1}
def trace(self):
line_l, line_r, red, yellow, green = self.line_detector.detect_lines()
self.r, self.y, self.g = red, yellow, green
self.line_detector.show_images(line_l, line_r)
self.information["roll"], self.information["pitch"], self.information["yaw"] = self.imu.get_data()
angle = self.steer(line_l, line_r)
speed = self.accelerate(angle, red, yellow, green)
self.information["speed"] = speed + 90
self.driver.drive(angle + 90, speed + 90)
print(speed + 90)
print(r, p, y)
def steer(self, left, right):
if left == -1:
if 320 < right < 390:
angle = -50
else:
angle = (550 - right) / (-3)
elif right == -1:
if 250 < left < 320:
angle = 50
else:
angle = (left - 90) / 3
else:
angle = 0
return angle
def accelerate(self, angle, red, yellow, green):
# if red is detected, stop
if red and (not yellow) and (not green):
speed = 0
# if yellow is detected, go low speed
elif (not red) and yellow and (not green):
speed = 20
# if green is detected, go origin speed
elif (not red) and (not yellow) and green:
speed = 50
# This is origin code
else: # (not red) and (not yellow) and (not green):
if angle <= -40 or angle >= 40:
speed = 30
elif angle <= -25 or angle >= 25:
speed = 40
else:
speed = 50
print(red, yellow, green)
return speed
def printInformation(self):
if self.r:
self.information["signal"] = "red"
self.information["=>"] = "stop!"
if self.y:
self.information["signal"] = "yellow"
self.information["=>"] = "slow"
if self.g:
self.information["signal"] = "green"
self.information["=>"] = "just go"
print("speed : " + str(self.information["speed"]) + " ", "signal : " + str(self.information["signal"]) + " ",
"=> " + str(self.information["=>"]))
print("pitch : " + str(self.information["pitch"]) + " ", "roll : " + str(self.information["roll"]) + " ",
"yaw : " + str(self.information["yaw"]))
print("")
self.r, self.y, self.g = False, False, False
def exit(self):
print('finished')
class AutoDrive:
def __init__(self):
rospy.init_node('xycar_driver')
self.line_detector = LineDetector('/usb_cam/image_raw')
self.obstacle_detector = ObstacleDetector('/ultrasonic')
self.driver = MotorDriver('/xycar_motor_msg')
def trace(self):
obs_l, obs_m, obs_r = self.obstacle_detector.get_distance()
line_l, line_r = self.line_detector.detect_lines()
self.line_detector.show_images(line_l, line_r)
angle = self.steer(line_l, line_r)
speed = self.accelerate(angle, obs_l, obs_m, obs_r)
self.driver.drive(angle + 90, speed + 90)
def steer(self, left, right):
mid = (left + right) // 2
angle = 0
if 600 <= right < 650:
if mid > 360:
angle -= 20
else:
angle += 0
elif 510 <= right < 600:
angle -= 30
if mid < 280:
angle -= 10
else:
angle -= 40
if -10 <= left < 65:
if mid < 280:
angle += 20
else:
angle += 0
elif 65 <= left < 120:
angle += 30
if mid > 360:
angle += 10
else:
angle += 40
print(
"reft",
left,
"mid",
mid,
"right",
right,
)
print("")
return angle
def accelerate(self, angle, left, mid, right):
if min(left, mid, right) < 50:
speed = 0
if angle < -20 or angle > 20:
speed = 20
else:
speed = 30
return speed
def exit(self):
print('finished')
class AutoDrive:
def __init__(self):
rospy.init_node('xycar_driver')
self.line_detector = LineDetector('/usb_cam/image_raw')
self.obstacle_detector = ObstacleDetector('/ultrasonic')
self.driver = MotorDriver('/xycar_motor_msg')
self.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.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):
self.image_width = 640 # 이미지의 너비
rospy.init_node('xycar_driver')
self.driver = MotorDriver('/xycar_motor_msg')
self.cam_image = CamImage('/usb_cam/image_raw')
self.pub = rospy.Publisher('/remote_rec', Image, queue_size=1)
rospy.Subscriber('/remote_pub', Int32MultiArray, self.get_yolo_result)
rospy.Subscriber('/xycar_ad_controller/controller_msg', Int32MultiArray, self.get_controller_msg)
self.cont_pub = rospy.Publisher('/xycar_ad/controller_msg', Int32MultiArray, queue_size=1)
self.yolo_dat = None
self.x = 50
self.y = 50
self.dir = 0
self.dx = self.x
self.dy = self.y
self.shortest_path = SPF(100, 100)
def get_controller_msg(self, loc):
self.dx = loc.data[0]
self.dy = loc.data[1]
def go_forward(self):
self.driver.drive(90, 90 + 30)
time.sleep(2)
self.driver.drive(90, 90)
def get_shortest_path(self):
return self.shortest_path.get_path(self.x, self.y, self.dx, self.dy)
def add_obstacle(self, x, y):
self.shortest_path.set_obs(x, y)
def get_yolo_result(self, data):
print('recieved')
self.yolo_dat = data.data
if self.yolo_dat[0] == 1:
print('detected!', self.yolo_dat[1:])
else:
print('not detected')
#print(data.data)
def publish_to_remote(self):
#pub_info = [1, 1]
#pub_info = Int32MultiArray(data=pub_info)
dat = self.cam_image.getData()
self.pub.publish(dat)
def for_bck(self):
for stop_cnt in range(2):
self.driver.drive(90, 90)
time.sleep(0.1)
self.driver.drive(90, 70)
time.sleep(0.1)
def turn(self, ccw=False):
angle = 50
if ccw:
angle *= -1
self.driver.drive(90, 90 + 30)
time.sleep(0.3)
for i in range(5):
self.turn_(angle)
time.sleep(0.5)
#self.bwd_stop()
self.driver.drive(90, 90 - 20)
print('back')
time.sleep(0.5)
self.bwd_stop()
self.driver.drive(90, 90)
def turn_(self, angle):
self.driver.drive(90 + angle, 90 + 30)
time.sleep(0.5)
self.fwd_stop()
time.sleep(0.5)
self.for_bck()
self.driver.drive(90 - angle, 90 - 25)
time.sleep(0.5)
self.bwd_stop()
time.sleep(0.5)
def fwd_stop(self):
self.driver.drive(90, 40)
time.sleep(0.1)
self.driver.drive(90, 90)
def bwd_stop(self):
self.driver.drive(90, 140)
time.sleep(0.1)
self.driver.drive(90, 90)
class AutoDrive:
def __init__(self):
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')
def __del__(self):
MotorDriver.__del__(self) # call MotorDriver class destructor
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')
def setPose(self, x, y):
MotorDriver.resetWristHeight(
self) # set linear actuator to home position
qBase, qElbow, qWrist = self.sLineTraj(
x, y) # get straight-line trajectory
#___drive motors with trajectory arrays_______#
for i in range(len(qBase)): #
MotorDriver.setBaseAngle(self, qBase[i]) #
MotorDriver.setElbowAngle(self, qElbow[i]) #
MotorDriver.setWristAngle(self, qWrist[i]) #
#---------------------------------------------#
#___move linear actuator closer to the connect 4 game board___#
if (x < -0.14): #
MotorDriver.setWristHeight(self, WRIST_TO_BOARD_HEIGHT + 15) #
else: #
MotorDriver.setWristHeight(self, WRIST_TO_BOARD_HEIGHT) #
#-------------------------------------------------------------#
# input() # uncomment to stop the robot arm when the EE reaches the game board
MotorDriver.resetWristHeight(
self) # reset linear actuator to home position
#___if current position is not home position, move the robot back to home position___#
if ((self.curPos[0] != self.HOME_POS[0])
and (self.curPos[1] != self.HOME_POS[1])): #
qBase, qElbow, qWrist = self.sLineTraj(self.HOME_POS[0],
self.HOME_POS[1]) #
#
for i in range(len(qBase)): #
MotorDriver.setBaseAngle(self, qBase[i]) #
MotorDriver.setElbowAngle(self, qElbow[i]) #
MotorDriver.setWristAngle(self, qWrist[i]) #
time.sleep(0.2) #
#------------------------------------------------------------------------------------#
return qBase, qElbow, qWrist # return last trajectory calculated, not really required at the moment
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.speed = 0
self.speed2 = 0
# 장애물, 차선 인식하여 조향각, 속도 결정, 모터 제어 메시지 발생
def trace(self):
obs_l, obs_m, obs_r = self.obstacle_detector.get_distance()
left, right = self.line_detector.detect_lines()
self.len30, self.len40, self.len50 = self.line_detector.img_match()
print(self.len30, self.len40, self.len50)
point = self.line_detector.show_images(left, right) # 디버깅위한 화면 표시
angle = self.steer(left, right)
speed = self.accelerate(angle, obs_l, obs_m, obs_r, point) # 조향각과 속도 인자 결정
self.driver.drive(angle + 90, speed + 90) # 모터 제어를 위한 메서드 호출
# 왼쪽/오른쪽 차선 위치에 따라 조향각 결정
# 가운데 : 0, 오른쪽 : 양수, 왼쪽 : 음수
def steer(self, left, right):
angle = 0
mid = (left + right) // 2
angle = int((mid - 320) * 0.5)
if angle >= 30:
angle = 50
elif angle <= -30:
angle = -50
return angle
# 조향각, 장애물 거리에 따라 속도 결정
def accelerate(self, angle, left, mid, right, point):
if self.len30 > 30:
self.speed = 5
print("speed is 30")
elif self.len40 > 20:
self.speed = 10
print("speed is 40")
elif self.len50 > 15:
self.speed = 15
print("speed is 50")
if min(left, mid, right) < 100:
speed = min(left, mid, right) - 60
if speed < 10:
speed = 0
elif angle < -20 or angle > 20:
speed = 0
else:
speed = 0 # 40
if len(point) == 0:
pass
else:
if point[1] >= 155 and point[2] >= 155:
pass
elif 180 <= point[2]:
print("red")
self.speed2 = 0
self.speed = 0
elif 150 <= point[1]:
print("green")
self.speed2 = 15
return self.speed2 + self.speed
def exit(self):
print('finished')
