def process_subcam(image):
""" process the subcam image """
if not SCHEDULER.is_subcam_enable():
return
if EYE.is_sub_occupied() or TURTLE.is_occupied():
return
if SCHEDULER.debug_option["show_timer"]:
SCHEDULER.check_time("subcam", min=0.25)
info = EYE.see_sub(image)
if info is None:
# rospy.logwarn("[PROC] No Information!")
return
center = info["center"]
slope = info["slope"]
if slope < -0.5:
# limit = 1.6
limit = 1.5 # For slow speed
amplitude = 1.6
# elif slope > 0.5:
# limit = 1.6
# amplitude = 1.2
else:
# limit = 1.2
# amplitude = 1.2
limit = 1.2 # For slow speed
amplitude = 1.2 # For slow speed
if EYE.get_front_state() == "straight":
if (abs(center) < 30) and slope < -0.4:
degree = pow(abs(slope) / 1.8, 1.1) * amplitude
elif center < 0:
degree = pow(abs(center) / 100, 2.0) * amplitude
elif center > 0:
degree = - pow(abs(center) / 100, 2.0) * amplitude
else:
degree = 0
else:
if (abs(center) < 30) and slope < -0.4:
degree = pow(abs(slope) / 1.8, 0.9) * amplitude
elif center < 0:
degree = pow(abs(center) / 100, 1.9) * amplitude
elif center > 0:
degree = - pow(abs(center) / 100, 1.9) * amplitude
else:
degree = 0
buf_sum = sum(BUF_ANGULAR)
if EYE.get_front_state() == "straight":
adjust_angular = BUF_ANGULAR.pop(0) * 0.9
BUF_ANGULAR.append(degree)
degree -= adjust_angular
# if abs(buf_sum) > 1:
else:
reset_buffer()
adjust_angular = 0
degree = max(min(degree, limit), -limit)
if not info["has_line"]:
if center < -55:
# degree = 1.6
degree = 1.4 # For slow speed
elif center > 50:
degree = -1.2
# degree = -1.2 # For slow speed
elif center > 10: # editing...
degree = -0.9 # editing...
else:
degree = 1.4
# if SCHEDULER.debug_option["show_center_slope"]:
# rospy.logdebug(
# "[PROC] center: {:.2f} slope: {:.2f} degree: {:.2f} adj: {:.2f} buf_sum: {:.2f} {} {}".format(
# center, slope, degree, adjust_angular, buf_sum, EYE.get_front_state(), info["has_line"])
# )
rospy.Timer(
rospy.Duration(0.15), EYE.release_sub_occupied, oneshot=True
)
TURTLE.turn(0.13, degree)
def process_subcam(image):
""" process the subcam image """
if not SCHEDULER.is_subcam_enable():
return
if SCHEDULER.is_subcam_occupied() or TURTLE.is_occupied():
return
info = EYE.see_sub(image)
if SCHEDULER.debug_option["show_timer"]:
# Check delay only if has line
# SCHEDULER.check_time("subcam", min=0.28, stop_when_delay=info["has_line"])
SCHEDULER.check_time("subcam", min=0.28, stop_when_delay=False)
if info is None:
rospy.logwarn("[PROC] No Information!")
return
center = info["center"]
slope = info["slope"]
if slope < -0.5:
limit = 1.6
amplitude = 1.0
else:
limit = 1.2
amplitude = 0.8
limit /= 1.9
# amplitude /= 2
state = SCHEDULER.get_state()
if (EYE.get_front_state() == "straight") and (state is not "zigzag"):
if (abs(center) < 30) and slope < -0.4:
degree = pow(abs(slope) / 1.8, 1.1) * amplitude
elif center < 0:
degree = pow(abs(center) / 100, 2.0) * amplitude / 2
elif center > 0:
degree = -pow(abs(center) / 100, 2.0) * amplitude
else:
degree = 0
else:
if slope < 0:
degree = -pow(abs(slope) / 1.8, 2.9) * amplitude * 28.0
else:
degree = pow(abs(slope) / 1.0, 1.2) * amplitude * 4.0
# degree = pow(abs(slope) / 1.8, 0.9) * amplitude
# elif center < 0:
# degree = pow(abs(center) / 100, 1.9) * amplitude
# elif center > 0:
# degree = - pow(abs(center) / 100, 1.9) * amplitude
# else:
# degree = 0
buf_sum = sum(BUF_ANGULAR)
if EYE.get_front_state() == "straight":
adjust_angular = BUF_ANGULAR.pop(0) * 0.9
BUF_ANGULAR.append(degree)
degree -= adjust_angular
# if abs(buf_sum) > 1:
else:
reset_buffer()
adjust_angular = 0
degree = max(min(degree, limit), -limit)
if not info["has_line"]:
if center > 200:
# degree = -1.2
degree = -1.1 # For enhancing frequency
# elif EYE.get_front_state() == "straight":
# degree = 0.6
else:
# degree = 1.4
degree = 1.3 # For enhancing frequency
# if not info["has_line"]:
# if center < -55:
# # degree = 1.6
# degree = 1.4 # For slow speed
# elif center > 50:
# degree = -1.2
# # degree = -1.2 # For slow speed
# # elif center > 19:
# # degree = -1.2
# else:
# degree = 1.4
if SCHEDULER.debug_option["show_center_slope"]:
rospy.logdebug(
"[PROC] center: {:.2f} slope: {:.2f} degree: {:.2f} adj: {:.2f} buf_sum: {:.2f} {} {}"
.format(center, slope, degree, adjust_angular, buf_sum,
EYE.get_front_state(), info["has_line"]))
rospy.Timer(rospy.Duration(0.15),
SCHEDULER.release_subcam_occupied,
oneshot=True)
TURTLE.turn(0.13, degree)
