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