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