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