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_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_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