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