def process_eye(image):
""" process the Eye System """
global TRACKING
global TURNING_TO
# if TRACKING is not "front":
# return
# rospy.loginfo('\n[PROC] frontcam image received')
# rospy.loginfo(EYE.is_occupied())
if not SCHEDULER.is_frontcam_occupied():
info = EYE.see(image)
# rospy.loginfo("\n[PROC] info: " + str(info))
rospy.Timer(rospy.Duration(0.1), EYE.release_occupied, oneshot=True)
if TRACKING is not "front":
return
if info is None:
pass
else:
center = info["center"]
TURTLE.set_speed('normal')
if info["state"] is "straight":
# TURTLE.set_speed('normal')
pass
elif info["state"] is "lost_track":
rospy.loginfo("\n[PROC] state: lost_track")
TURNING_TO = info["turning_to"]
TURTLE.go_forward(1)
rospy.Timer(rospy.Duration(1),
track_fish,
oneshot=True,
reset=True)
# elif info["state"] is "turning":
# TURNING_TO = info["turning_to"]
# track_fish()
# rospy.Timer(rospy.Duration(5.5), track_fish, oneshot=True, reset=True)
if center > 500:
TURTLE.set_angular(0.27)
rospy.loginfo("\n[PROC] [!] turning to right at " +
str(center))
return
if center < -500:
TURTLE.set_angular(-0.27)
rospy.loginfo("\n[PROC] [!] turning to left at " + str(center))
return
if center > 60:
TURTLE.set_angular(0.12)
# TURTLE.set_angular(0.08)
rospy.loginfo("\n[PROC] turning to right at " + str(center))
return
elif center < -60:
TURTLE.set_angular(-0.12)
# TURTLE.set_angular(-0.08)
rospy.loginfo("\n[PROC] turning to left at " + str(center))
return
TURTLE.set_angular_smooth(0)
return
def process_subcam(image):
""" process the subcam image """
start = timeit.default_timer()
global LINE_BASE
global TRACKING
global TURNING_TO
global TEST_ANGULAR
if TRACKING is not "fish":
return
if not SCHEDULER.is_subcam_occupied():
# if True:
rospy.Timer(rospy.Duration(0.04),
SCHEDULER.release_subcam_occupied,
oneshot=True)
info = EYE.see_sub(image)
# rospy.Timer(rospy.Duration(0.04), SCHEDULER.release_subcam_occupied, oneshot=True, reset=True)
if info is None:
print("NO INFO!")
# TURTLE.set_angular_smooth(0.12)
# pass
else:
# TURTLE.set_speed('slow')
# if info["slope"]:
# TURTLE.set_speed_by_percentage(-abs(info["slope"] / 6))
# else:
TURTLE.set_speed('normal')
center_x, center_y, point_x, point_y = (
value for value in info["line_center"])
gap_x = abs(point_x - center_x)
rospy.loginfo("\n[PROC] info: \n" + str(info))
rospy.loginfo("\nGAP: " + str(gap_x))
if info["slope"] > 0:
slope = math.sqrt(0.5 * info["slope"])
else:
slope = -math.sqrt(-0.5 * info["slope"])
if not info["has_line"]:
if gap_x > 100:
if TEST_ONCE:
TURTLE.set_angular(-1.75)
TURTLE.set_angular_smooth(-0.1)
else:
TURTLE.set_angular_smooth(-0.1)
elif gap_x < 0:
if TEST_ONCE:
TURTLE.set_angular(1.75)
TURTLE.set_angular_smooth(0.2)
else:
TURTLE.set_angular_smooth(0.2)
TEST_ONCE = False
# elif gap_x > 100:
# TURTLE.set_angular(-0.15 * info["slope"] * 4)
# TURTLE.set_angular_smooth(-0.1)
else:
TEST_ONCE = True
if gap_x > 100:
TURTLE.set_angular(-0.15 + slope)
TURTLE.set_angular_smooth(-0.1)
elif gap_x > 30:
TURTLE.set_angular(0)
else:
# if slope < 0:
# TURTLE.set_angular(-0.25 + slope * 1.5)
# TURTLE.set_angular_smooth(-0.1)
# else:
rospy.loginfo("LOST")
TURTLE.set_angular(0.45 + slope)
TURTLE.set_angular_smooth(0.1)
# SCHEDULER.release_subcam_occupied()
end = timeit.default_timer()
# print("sub l: {:d}".format(info["left"]) + " s: {:.01f}".format(slope)
# + " time: {:.02f}".format(end - start))
print("\nTIME: {:.02f}".format(end - start))
