def process_acceleration(info): if not info: return horizon = info['horizon_position'] center = abs(info['center']) if EYE.get_front_state() == "straight": if horizon < 200 and center < 50: TURTLE.boost() if SCHEDULER.debug_option["show_front_info"]: rospy.logwarn("[PROC] Boosting") elif horizon < 260 and center < 50: TURTLE.set_speed_smooth('little') if horizon > 320 or info['state'] == 'turning': TURTLE.set_speed('normal') elif horizon > 260: TURTLE.set_speed_smooth('normal')
def process_frontcam(image): """ process the frontcam image """ if not SCHEDULER.is_frontcam_enable(): return STATE = SCHEDULER.get_state() info = EYE.see_front(image) if SCHEDULER.debug_option["show_front_info"]: rospy.logdebug(info) if STATE == "default": # if EYE.is_boostable(image): process_acceleration(info) # signal = is_construction(image) # rospy.logdebug(signal) if STATE == "traffic_light": if is_light_green(image): TURTLE.enable() SCHEDULER.set_state("to_intersection") return if STATE == "to_intersection": signal = check_left_right_sign(image) if signal == "right": SCHEDULER.set_state("intersection_right") elif signal == "left": SCHEDULER.set_state("intersection_left") return if STATE == "intersection_right": # TODO: make algorithms for right if EYE.is_boostable(image): TURTLE.boost() SCHEDULER.set_state("to_construction") return if STATE == "intersection_left": if EYE.is_boostable(image): TURTLE.boost() SCHEDULER.set_state("to_construction") return if STATE == "to_construction": if EYE.is_boostable(image): TURTLE.boost() if is_construction(image): SCHEDULER.set_state("construction_searching") if STATE == "construction_searching": pass
def process_frontcam(image): """ process the frontcam image """ if not SCHEDULER.is_frontcam_enable(): return state = SCHEDULER.get_state() info = EYE.see_front(image) if SCHEDULER.debug_option["show_front_info"]: rospy.logdebug(info) if state == "default": # if EYE.is_boostable(image): process_acceleration(info) # SCHEDULER.set_state("to_intersection") # signal = is_construction(image) # rospy.logdebug(signal) # if is_construction(image): # TURTLE.boost() # SCHEDULER.set_state("construction") if state == "traffic_light": if is_light_green(image): TURTLE.enable() SCHEDULER.set_state("to_intersection") return # NOTE: temporary settings: if state == "to_intersection": # rospy.Timer(rospy.Duration(35), SCHEDULER.enable_lidar, oneshot=True) SCHEDULER.set_state("intersection_left") # if state == "to_intersection": # signal = check_left_right_sign(image) # if signal == "right": # SCHEDULER.set_state("intersection_right") # elif signal == "left": # SCHEDULER.set_state("intersection_left") # return if state == "intersection_right": # TODO: make algorithms for right if EYE.is_boostable(image): TURTLE.boost() SCHEDULER.set_state("to_construction") return # NOTE: temporary settings: if state == "intersection_left": # if EYE.is_boostable(image): # TURTLE.boost() # SCHEDULER.set_state("to_construction") return if state == "to_construction": # if EYE.is_boostable(image): # TURTLE.boost() EYE.check_yellow = True # if is_construction(image): # SCHEDULER.set_state("construction_searching") if state == "construction_searching": pass