def view_frontcam(image):
""" process the frontcam """
# rospy.loginfo("[VIEW] frontcam image received.")
if not EYE.is_front_occupied():
if SCHEDULER.debug_option["show_center_slope"]:
SCHEDULER.check_time("frontcam", min=0.4)
info = EYE.see_front(image)
if info is None:
return
# rospy.logdebug("info: {:s}".format(str(info)))
if info["center"] is "-1000" or info["center"] is "1000" or info["center"] is "0":
pass
else:
EYE.reset_state()
rospy.Timer(rospy.Duration(0.1),
EYE.release_front_occupied, oneshot=True)
def test_frontcam(image):
""" process the frontcam image """
# print("image is subscribed")
if not SCHEDULER.is_frontcam_enable():
return
if not SCHEDULER.is_frontcam_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),
# SCHEDULER.release_frontcam_occupied, oneshot=True)
# else:
rospy.Timer(rospy.Duration(0.1),
SCHEDULER.release_frontcam_occupied,
oneshot=True)
def process_acceleration(info):
if not info:
return
horizon = info['horizon_position']
center = abs(info['center'])
if EYE.get_front_state() == "straight":
if horizon < 220 and center < 50:
# TURTLE.boost()
TURTLE.set_speed_smooth('little')
if SCHEDULER.debug_option["show_front_info"]:
rospy.logwarn("[PROC] Boosting")
elif horizon < 280 and center < 150:
TURTLE.set_speed_smooth('normal')
if horizon > 210 or info['state'] == 'turning':
TURTLE.set_speed_smooth('normal')
elif horizon > 290:
TURTLE.set_speed('normal')
def view_subcam(image):
""" process the subcam image """
global DIRECTION
global BUF_ANGULAR
if not SCHEDULER.is_enable["subcam"]:
return
if SCHEDULER.is_subcam_occupied():
return
if SCHEDULER.debug_option["show_timer"]:
SCHEDULER.check_time("subcam", min=0.3)
info = EYE.see_sub(image)
# print(EYE.get_front_state() + " " + str(EYE.get_front_state()))
if info is None:
rospy.logwarn("[PROC] No Information!")
return
elif False:
# elif EYE.get_front_state() is "turning":
# rospy.logdebug("[PROC] turning...")
center = info["center"]
slope = info["slope"]
# left: + right: -
# if slope > 0:
# weight_slope = pow(abs(slope) / 1.8, 0.9) * 2.6
# else:
# weight_slope = - pow(abs(slope) / 1.8, 0.9) * 2.6
# # weight_slope = - pow(abs(slope) / 1.8, 0.5) * 2.5
if slope > 0:
value = pow(abs(slope) / 1.8, 1.2) * 3.2
else:
value = -pow(abs(slope) / 1.8, 1.2) * 3.2
# if slope > 0:
# weight_center = pow(abs(center) / 250, 0.9) * 5.5
# elif slope < -0:
# weight_center = - pow(abs(center) / 250, 0.9) * 5.5
# else:
# weight_center = 0
# # weight_center = slope * 1
if value > 2.6:
value = 2.6
elif value < -2.6:
value = -2.6
degree = value
# BUF_ANGULAR.append(value)
# past_val = BUF_ANGULAR.pop(0) * 0.7
# if info["has_line"]:
# if (value * past_val >= 0) and (abs(value) > abs(past_val)):
# degree = value
# else:
# degree = 0
# BUF_ANGULAR = [0] * BUF_SIZE
# else:
# if value > 0:
# degree = 2.7
# elif value < 0:
# degree = -2.7
# else:
# degree = 0
# if not info["has_line"]:
# if (slope < 0):
# degree = 4.12
# else:
# degree = -4.12
if SCHEDULER.debug_option["show_center_slope"]:
rospy.logdebug(
"[PROC] slope: {:.2f} w_slope: {:.2f} degree: {:.2f} {}".
format(slope, value, degree, info["has_line"]))
elif EYE.get_front_state() is "straight":
# rospy.logdebug("[PROC] going straight...")
center = info["center"]
if center < 0:
value = pow(abs(center) / 150, 0.9) * 2
elif center > 0:
value = -pow(abs(center) / 150, 0.9) * 2
else:
value = 0
if value > 1.5:
value = 1.5
elif value < -1.5:
value = -1.5
degree = value
if SCHEDULER.debug_option["show_center_slope"]:
rospy.logdebug(
"[PROC] center: {:.2f} w_center: {:.2f} {}".format(
center, degree, info["has_line"]))
rospy.Timer(rospy.Duration(0.14),
SCHEDULER.release_subcam_occupied,
oneshot=True)
# TURTLE.turn("", 0.13, degree)
SCHEDULER.release_subcam_occupied()
def initialize():
""" initialize processing """
EYE.calibrate()
def calibrate_subcam(image):
""" calibrate the subcam """
info = EYE.see_sub(image)
rospy.loginfo("info: {:s}".format(str(info)))
rospy.signal_shutdown("[VIEW] ended calibration")
#!/usr/bin/env python3
# import rospy
# from sensor_msgs.msg import CompressedImage
# import processor
# from turtlebot import TURTLE
# from constants import PATH_USBCAM, IS_DEBUG_MODE
from lib_eye import EYE
# TODO: Fix this: (variable of constants.py will never be changed)
IS_DEBUG_MODE = True
i = 0
while i <= 50:
image_path = "../../../assets/images_usb/image" + "{:02d}".format(
i) + ".png"
key = EYE.see(image_path=image_path)
i = i + key
if i < 0 or i > 50:
i = 0
# rospy.Subscriber(PATH_USBCAM, CompressedImage, processor.process_eye, queue_size=1)
# while not rospy.is_shutdown():
# TURTLE.move()
# rospy.Rate(120).sleep()
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
def initialize():
""" initialize processing """
EYE.calibrate()
init_ref_images()
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)