def test_parking(image):
global STEP
if not TURTLE.is_occupied():
if STEP == 1:
TURTLE.go_forward(0.5)
elif STEP == 2:
TURTLE.set_speed("normal")
TURTLE.go_turn("right", 2)
elif STEP == 3:
TURTLE.go_forward(0.7)
# for stopping for a while
rospy.sleep(rospy.Duration(0.5))
elif STEP == 4:
TURTLE.go_backward(1)
elif STEP == 5:
TURTLE.go_turn_backward(1.2)
elif STEP == 6:
TURTLE.go_forward(2)
elif STEP == 7:
TURTLE.set_speed("fast")
TURTLE.set_speed_smooth("normal")
# TURTLE.set_speed("normal")
else:
return
STEP += 1
def process_construction():
""" process construction state """
global STEP
global NUM_OBSTRUCTION
global LANE_TO
if TURTLE.is_occupied():
return
if STEP == 0:
leftside = get_object_distance("leftside")
left = get_object_distance("left")
if leftside > 0:
rospy.logdebug("[PROC] LIDAR LEFTSIDE: {}".format(
leftside))
if (leftside > 0) and (leftside < 0.40):
STEP = 1
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
SCHEDULER.disable_cams()
rospy.loginfo("[PROC] construction state started.")
TURTLE.go_forward(3.5)
rospy.sleep(rospy.Duration(0.5))
return
else:
return
# elif (left > 0) and (left < 1.5):
# STEP = 2
# rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
# return
# rospy.sleep(rospy.Duration(2))
elif STEP == 1:
TURTLE.set_speed("normal")
TURTLE.set_speed_smooth("slow")
left = get_object_distance("left")
if left > 0:
rospy.logdebug("[PROC] LIDAR LEFT: {}".format(
left))
if (left < 0.50) or (left > 1.5):
return
else:
STEP = 3
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
return
elif STEP == 2:
# TODO: write code for first left lane
pass
elif STEP == 3:
TURTLE.go_turn("left", speed=0.11)
LANE_TO = "left"
elif STEP == 4:
TURTLE.set_speed("normal")
reverse_lane()
biased = get_object_distance(LANE_TO + "_biased")
if biased > 0:
rospy.logdebug("[PROC] LIDAR {:s}_BIASED: {}"
.format(LANE_TO, biased))
reverse_lane()
if (biased == 0) or (biased > 0.30):
return
TURTLE.go_turn(LANE_TO, duration=0.5, angular=4.2)
TURTLE.set_speed("normal")
# TURTLE.set_speed("fast")
# if LANE_TO is "left":
# TURTLE.turn(0.13, 1.3, consuming_time=1.5)
# else:
# TURTLE.turn(0.13, -1.3, consuming_time=1.5)
rospy.sleep(rospy.Duration(2.2))
reverse_lane()
elif STEP == 5:
TURTLE.go_turn(LANE_TO, duration=0.7, angular=4.2)
TURTLE.set_speed("normal")
# TURTLE.set_speed("fast")
# if LANE_TO is "left":
# TURTLE.turn(0.13, 1.3, consuming_time=1.5)
# else:
# TURTLE.turn(0.13, -1.3, consuming_time=1.5)
NUM_OBSTRUCTION += 1
if NUM_OBSTRUCTION < 2:
STEP = 4
return
elif STEP == 6:
TURTLE.go_forward(1)
rospy.sleep(rospy.Duration(0.6))
elif STEP == 7:
TURTLE.go_turn("left", duration=0.8, angular=3)
elif STEP == 8:
TURTLE.set_speed("fast")
TURTLE.set_speed_smooth("normal")
TURTLE.go_forward(5)
rospy.sleep(rospy.Duration(0.5))
elif STEP == 9:
# NOTE: turn to parking step
STEP = 10
SCHEDULER.set_state("parking")
else:
return
STEP += 1
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
def process_construction_new():
""" process construction state """
global STEP
global NUM_OBSTRUCTION
global LANE_TO
global BUF_ANGULAR
global BUF_SIZE
if TURTLE.is_occupied():
return
if STEP == 0:
# TURTLE.set_speed("normal")
leftside = get_object_distance("leftside")
left = get_object_distance("left")
if leftside > 0:
rospy.logdebug("[PROC] LIDAR LEFTSIDE: {}".format(leftside))
if (leftside > 0) and (leftside < 0.50) and (left > 1.00):
EYE.check_yellow = False
SCHEDULER.set_state("construction")
rospy.loginfo("[PROC] construction state started.")
STEP = 1
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
TURTLE.go_forward(3.5)
return
else:
return
elif STEP == 1:
TURTLE.set_speed("normal")
TURTLE.set_speed_smooth("slow")
TURTLE.turn(0.13, 0)
left = get_object_distance("left")
leftback = get_object_distance("leftback")
rospy.logdebug("[PROC] LIDAR LEFT: {:.2f} LEFTBACK: {:.2f}}".format(
left, leftback))
if (left > 0) and (left < 0.50):
return
else:
TURTLE.set_speed("slow")
if (leftback > 0.5):
# TURTLE.go_forward(2.5)
STEP = 3
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
return
elif STEP == 2:
# TODO: write code for first left lane
pass
elif STEP == 3:
TURTLE.set_speed("normal")
TURTLE.set_speed_smooth("stop")
front = get_object_distance("front")
right_biased = get_object_distance("right_biased")
if (front > 1.0) and (right_biased < 1.0) and (right_biased > 0.0):
print("passed", front, right_biased)
print("BUF_SIZE: ", BUF_SIZE)
BUF_SIZE = 15
reset_buffer()
TURTLE.set_speed_smooth("slow")
pass
elif front * right_biased == 0:
return
else:
TURTLE.set_speed("stop")
print("turning...", front, right_biased)
TURTLE.turn(0.13, 1.0)
# rospy.sleep(rospy.Duration(5.0))
return
# TURTLE.go_turn("left")
elif STEP == 4:
right_biased = get_object_distance("right_biased")
left_biased = get_object_distance("left_biased")
front = get_object_distance("front")
if right_biased == 0.0:
right_biased = 3.0
if left_biased == 0.0:
left_biased = 3.0
if front == 0.0:
front = 3.0
elif front < 0.2:
TURTLE.set_speed_smooth("stop")
else:
TURTLE.set_speed_smooth("slow")
min_distance = min(right_biased, left_biased)
degree = 0
if (front < 1.0):
degree += max(pow(1.0 - front, 2), 0)
else:
degree += max(0.5 - min_distance, 0) * 3
# if min_distance < 0.5:
# degree += max((0.5 - min_distance), 0) * 1.5
# elif (min_distance > 1.0) and (min_distance < 3.0):
# degree = 0.2
if (left_biased == min_distance) and (min_distance < 0.5):
degree *= -1
# max_distance = max(right_biased, left_biased)
# if (left_biased == max_distance):
# degree *= -1
# degree = 0
# if min_distance > 0 and min_distance < 0.5:
# if right_biased > left_biased:
# degree = (0.5 - min_distance) * (-7)
# LANE_TO = "right"
# elif right_biased < left_biased:
# degree = (0.5 - min_distance) * (7)
# LANE_TO = "left"
# if is_left_crashable():
# degree = -1.7
# elif is_right_crashable():
# degree = 1.7
degree *= 3
degree = max(min(degree, 2.0), -2.0)
BUF_ANGULAR.append(degree)
degree -= BUF_ANGULAR.pop(0)
print("BUF_ANGULAR:", BUF_ANGULAR)
# if degree != 0:
# BUF_ANGULAR.append(degree)
# elif len(BUF_ANGULAR) > 9:
# STEP = 5
if SCHEDULER.debug_option["show_construction_lidar"]:
rospy.logdebug(
"[PROC] r_based: {:.2f} l_based: {:.2f} min: {:.2f} front: {:.2f} deg: {:.2f}"
.format(right_biased, left_biased, min_distance, front,
degree))
TURTLE.turn(0.13, degree)
return
elif STEP == 5:
print("[StEP 5]")
if len(BUF_ANGULAR) > 0:
TURTLE.turn(0.13, -BUF_ANGULAR.pop(0))
return
else:
front = get_object_distance("front")
print(front)
# if (front > 0) and (front < 1.0):
# if LANE_TO == "right":
# TURTLE.turn(0.13, 0.8)
# else:
# TURTLE.turn(0.13, -0.8)
# return
# else:
if NUM_OBSTRUCTION < 1:
NUM_OBSTRUCTION += 1
STEP = 4
return
elif STEP == 6:
TURTLE.go_forward(1)
TURTLE.set_speed("normal")
TURTLE.go_turn("left")
TURTLE.set_speed("normal")
TURTLE.set_speed("fast")
TURTLE.set_speed_smooth("normal")
TURTLE.go_forward(5)
STEP = 10
SCHEDULER.set_state("parking")
BUF_SIZE = 3
reset_buffer()
return
STEP += 1
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
def process_parking():
""" process parking state """
global STEP
# if TURTLE.is_occupied():
# return
if STEP == 10:
frontleft = get_object_distance("frontleft")
frontright = get_object_distance("frontright")
if SCHEDULER.debug_option["show_parking_lidar"]:
rospy.logdebug("front: {:.2f} frontleft: {:.2f}".format(
get_object_distance("front"), get_object_distance("frontleft")
))
if (frontleft > 0) and (frontleft < 0.5):
STEP = 11
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
elif (frontright > 0) and (frontright < 0.5):
STEP = 12
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
# NOTE: return is needed to prevent executing STEP += 1
return
elif STEP == 11:
SCHEDULER.disable_cams()
TURTLE.set_speed("normal")
TURTLE.go_turn("right", 2)
STEP = 13
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
return
elif STEP == 12:
SCHEDULER.disable_cams()
TURTLE.set_speed("normal")
TURTLE.go_turn("left", 2)
STEP = 13
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
return
elif STEP == 13:
TURTLE.go_forward(0.7)
# for stopping for a while
rospy.sleep(rospy.Duration(0.5))
elif STEP == 14:
TURTLE.go_backward(1.0)
elif STEP == 15:
TURTLE.go_turn_backward(1.1)
elif STEP == 16:
TURTLE.set_speed("normal")
TURTLE.go_forward(1)
elif STEP == 17:
# TURTLE.set_speed("fast")
# TURTLE.set_speed_smooth("normal")
TURTLE.set_speed("normal")
SCHEDULER.set_state("zigzag")
else:
return
STEP += 1
def process_parking():
""" process parking state """
global STEP
# if TURTLE.is_occupied():
# return
if STEP == 10:
frontleft = get_object_distance("frontleft")
frontright = get_object_distance("frontright")
left = get_object_distance("left")
right = get_object_distance("right")
if SCHEDULER.debug_option["show_parking_lidar"]:
rospy.logdebug("frontright: {:.2f} frontleft: {:.2f}".format(
frontright, frontleft))
if (frontleft > 0) and (frontleft < 1.0):
STEP = 11
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
elif (frontright > 0) and (frontright < 1.0):
STEP = 12
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
if (left > 0) and (left < 0.5):
STEP = 13
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
elif (right > 0) and (right < 0.5):
STEP = 14
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
# NOTE: return is needed to prevent executing STEP += 1
return
elif STEP == 11:
SCHEDULER.disable_cams()
TURTLE.set_speed("normal")
TURTLE.go_forward(2.5)
TURTLE.go_turn("right", 2)
STEP = 15
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
return
elif STEP == 12:
# Edit HERE
return
elif STEP == 13:
# Edit HERE
return
elif STEP == 14:
SCHEDULER.disable_cams()
TURTLE.set_speed("normal")
TURTLE.go_turn("left", angular=1.8, duration=1.2)
STEP = 15
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
return
elif STEP == 15:
# Edit HERE
return
elif STEP == 16:
# Edit HERE
return
elif STEP == 17:
# Edit HERE
return
elif STEP == 18:
# Edit HERE
return
elif STEP == 19:
TURTLE.set_speed("fast")
SCHEDULER.set_state("zigzag")
else:
return
STEP += 1
