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 test_frontcam(image):
""" process the frontcam image """
# print("image is subscribed")
if not SCHEDULER.is_frontcam_enable():
return
if not EYE.is_front_occupied():
STATE = SCHEDULER.get_state()
if STATE == "traffic_light":
signal = is_light_green(image)
rospy.logdebug("[PROC] is_light_green: {}"
.format(signal))
if signal:
SCHEDULER.set_state("to_intersection")
TURTLE.enable()
TURTLE.set_speed("fast")
TURTLE.set_speed_smooth("normal")
elif STATE == "to_intersection":
signal = check_left_right_sign(image)
rospy.logdebug("[PROC] left or right: {}"
.format(signal))
if signal == "right":
SCHEDULER.set_state("intersection_right")
elif signal == "left":
SCHEDULER.set_state("intersection_left")
if SCHEDULER.debug_option["show_center_slope"]:
SCHEDULER.check_time("frontcam", min=0.5)
info = EYE.see_front(image)
if info is None:
return
# rospy.logdebug("[PROC] test_frontcam: state {} {}".format(
# info["state"], info["horizon_position"]))
# if (info["horizon_position"] < 150):
# TURTLE.set_speed("fast")
# TURTLE.set_speed_smooth("normal")
if (info["state"] == "turning") and (info["turning_to"] is not "None"):
rospy.logdebug(
"[PROC] turn off for 1.5s")
rospy.Timer(rospy.Duration(1.5),
EYE.release_front_occupied, oneshot=True)
else:
rospy.Timer(rospy.Duration(0.1),
EYE.release_front_occupied, oneshot=True)
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
