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