Ejemplo n.º 1
0
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
Ejemplo n.º 2
0
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