예제 #1
0
def draw_turn_Left(x,y,theta,v,direction,distance,radius):
    d = 0
    while(d <= distance):
        (x2,y2,theta2,v2) = util.turn_Left((x,y,theta,v),direction,d,radius)
        draw_point(x2,y2,theta2,'green')
        d+=10
    (x_temp,y_temp,theta_temp,v_temp) = util.turn_Left((x,y,theta,v),direction,distance,radius)
    draw_point(x_temp,y_temp,theta_temp,'green')
    return (x_temp,y_temp,theta_temp,v_temp)
예제 #2
0
def figureofeight((x1, y1, th1, v1)):

    path = []
    state = (x1, y1, th1, v1)
    newstate = None
    d = 0
    while d <= 2 * math.pi * parameters.ROBOT_RADIUS_MIN:
        newstate = util.turn_Left(state, 'f', 5, parameters.ROBOT_RADIUS_MIN)
        path.append((newstate[0] / 100, newstate[1] / 100, newstate[2]))
        state = newstate
        d += 5

    (x1, y1, th1, v1) = state
    newstate = None
    d = 0
    while d <= 2 * math.pi * parameters.ROBOT_RADIUS_MIN:
        newstate = util.turn_Right(state, 'f', 5, parameters.ROBOT_RADIUS_MIN)
        path.append((newstate[0] / 100, newstate[1] / 100, newstate[2]))
        state = newstate
        d += 5

    path_to_send = LatticePlannersim.Path()
    for point in path:
        pose = LatticePlannersim.Pose()
        # plan uses center of car, so transform such that path has points to be traversed by rear axle center,                                                      # which is 17.41 cm away from the center
        pose.x = point[0] - 17.41 * math.cos(point[2]) / 100.0
        pose.y = point[1] - 17.41 * math.sin(point[2]) / 100.0
        pose.theta = point[2]
        pathelement = lattice_planner.PathElement()
        pathelement.pose = pose
        pathelement.type = 't'
        pathelement.direction = 'f'
        path_to_send.poses.append(pathelement)

    lattice_planner.pub_path.publish(path_to_send)
    print "path published"
예제 #3
0
def figureofeight((x1,y1,th1,v1)):

    path = []
    state = (x1,y1,th1,v1)
    newstate = None
    d = 0
    while d <=  2*math.pi*parameters.ROBOT_RADIUS_MIN:
        newstate = util.turn_Left(state,'f',5,parameters.ROBOT_RADIUS_MIN)
        path.append((newstate[0]/100,newstate[1]/100,newstate[2]))
        state = newstate
        d += 5

    (x1,y1,th1,v1) = state
    newstate = None
    d = 0
    while d <= 2*math.pi*parameters.ROBOT_RADIUS_MIN:
        newstate = util.turn_Right(state,'f',5,parameters.ROBOT_RADIUS_MIN)
        path.append((newstate[0]/100,newstate[1]/100,newstate[2]))
        state = newstate
        d += 5
        
    path_to_send = LatticePlannersim.Path()
    for point in path:
        pose = LatticePlannersim.Pose()
        # plan uses center of car, so transform such that path has points to be traversed by rear axle center,                                                      # which is 17.41 cm away from the center                                                                                                                                                                                                                                                                     
        pose.x = point[0] - 17.41*math.cos(point[2])/100.0
        pose.y = point[1] - 17.41*math.sin(point[2])/100.0
        pose.theta = point[2]
        pathelement = lattice_planner.PathElement()
        pathelement.pose = pose
        pathelement.type = 't'
        pathelement.direction = 'f'
        path_to_send.poses.append(pathelement)
            
    lattice_planner.pub_path.publish(path_to_send)
    print "path published"
예제 #4
0
def segment(point,action,canv):
    """
    Given a state and an action, draw the segment corresponding to taking 'action' from 'state'
    """
    (x,y,theta) = point
    v = 0
    
    # Set direction of motion
    if action in ("R_f","L_f","F","F3","SR_f","SL_f","LS_f","RS_f","F_diag","F_diag3"):
        direction = "f"
    else:
        direction = "b"
    
    if action in ("R_f","R_b"): # turning right
        d = 0
        while(d <= util.ROBOT_RADIUS_MIN*math.pi/2):
            (x2,y2,theta2,v2) = util.turn_Right((x,y,theta,v),direction,d,util.ROBOT_RADIUS_MIN)
            car = draw_car(x2,y2,theta2,canv)            
            d+=10

    elif action in ("L_f","L_b"): # turning left
        d = 0
        while(d <= util.ROBOT_RADIUS_MIN*math.pi/2):
            (x2,y2,theta2,v2) = util.turn_Left((x,y,theta,v),direction,d,util.ROBOT_RADIUS_MIN)
            draw_car(x2,y2,theta2,canv)
            d +=10
    
    elif action in ("F","B"):
        d = 0
        while(d <= util.CELL_SIZE):
            (x2,y2,theta2,v2) = util.go_Straight((x,y,theta,v),direction,d)
            draw_car(x2,y2,theta2,canv)
            d +=10

    elif action in ("F3"):
        d = 0
        while(d <= 3*util.CELL_SIZE):
            (x2,y2,theta2,v2) = util.go_Straight((x,y,theta,v),direction,d)
            draw_car(x2,y2,theta2,canv)
            d +=10

    elif action == "SL_f":
        d = 0
        while(d <= 20.5/70.0*util.ROBOT_RADIUS_MIN):
            (x2,y2,theta2,v2) =  util.go_Straight((x,y,theta,v),direction,d)
            draw_car(x2,y2,theta2,canv)
            d += 10
        d = 0
        (x_temp,y_temp,theta_temp,v_temp) = util.go_Straight((x,y,theta,v),direction,20.5/70.0*util.ROBOT_RADIUS_MIN)
        while(d <= util.ROBOT_RADIUS_2*math.pi/4):
            (x2,y2,theta2,v2) = util.turn_Left((x_temp,y_temp,theta_temp,v_temp),direction,d,util.ROBOT_RADIUS_2)
            draw_car(x2,y2,theta2,canv)
            d += 10

    elif action == "SR_f":
        d = 0
        while(d <= 20.5/70.0*util.ROBOT_RADIUS_MIN):
            (x2,y2,theta2,v2) =  util.go_Straight((x,y,theta,v),direction,d)
            draw_car(x2,y2,theta2,canv)
            d += 10
        d = 0
        (x_temp,y_temp,theta_temp,v_temp) = util.go_Straight((x,y,theta,v),direction,20.5/70.0*util.ROBOT_RADIUS_MIN)
        while(d <= util.ROBOT_RADIUS_2*math.pi/4):
            (x2,y2,theta2,v2) = util.turn_Right((x_temp,y_temp,theta_temp,v_temp),direction,d,util.ROBOT_RADIUS_2)
            draw_car(x2,y2,theta2,canv)
            d += 10
    
    elif action == "LS_f":
        d = 0
        while(d <= util.ROBOT_RADIUS_2*math.pi/4):
            (x2,y2,theta2,v2) =  util.turn_Left((x,y,theta,v),direction,d,util.ROBOT_RADIUS_2)
            draw_car(x2,y2,theta2,canv)
            d += 10
        d = 0
        (x_temp,y_temp,theta_temp,v_temp) = util.turn_Left((x,y,theta,v),direction,util.ROBOT_RADIUS_2*math.pi/4,util.ROBOT_RADIUS_2)
        while(d <= 20.5/70.0*util.ROBOT_RADIUS_MIN):
            (x2,y2,theta2,v2) = util.go_Straight((x_temp,y_temp,theta_temp,v_temp),direction,d)
            draw_car(x2,y2,theta2,canv)
            d += 10

    elif action == "RS_f":
        d = 0
        while(d <= util.ROBOT_RADIUS_2*math.pi/4):
            (x2,y2,theta2,v2) =  util.turn_Right((x,y,theta,v),direction,d,util.ROBOT_RADIUS_2)
            draw_car(x2,y2,theta2,canv)
            d += 10
        d = 0
        (x_temp,y_temp,theta_temp,v_temp) = util.turn_Right((x,y,theta,v),direction,util.ROBOT_RADIUS_2*math.pi/4,util.ROBOT_RADIUS_2)
        while(d <= 20.5/70.0*util.ROBOT_RADIUS_MIN):
            (x2,y2,theta2,v2) = util.go_Straight((x_temp,y_temp,theta_temp,v_temp),direction,d)
            draw_car(x2,y2,theta2,canv)
            d += 10

    elif action == "F_diag":
        d = 0
        while(d <= util.CELL_SIZE*math.sqrt(2)):
            (x2,y2,theta2,v2) = util.go_Straight((x,y,theta,v),direction,d)
            draw_car(x2,y2,theta2,canv)
            d +=10
    
    elif action == "F_diag3":
        d = 0
        while(d <= 3*util.CELL_SIZE*math.sqrt(2)):
            (x2,y2,theta2,v2) = util.go_Straight((x,y,theta,v),direction,d)
            draw_car(x2,y2,theta2,canv)
            d +=10

    elif action == "B_diag":
        d = 0
        while(d <= util.CELL_SIZE*math.sqrt(2)):
            (x2,y2,theta2,v2) = util.go_Straight((x,y,theta,v),direction,d)
            draw_car(x2,y2,theta2,canv)
            d +=10
예제 #5
0
def draw_segment(state, action):
    """
    Given a state and an action, draw the segment corresponding to taking 'action' from 'state'
    """
    global canvas
    (x, y, theta, v) = state.get_stateparams()

    # Set direction of motion
    if action in ("R_f", "L_f", "F", "F3", "SR_f", "SL_f", "LS_f", "RS_f",
                  "F_diag", "F_diag3"):
        direction = "f"
    else:
        direction = "b"

    if action in ("R_f", "R_b"):  # turning right
        d = 0
        while (d <= util.ROBOT_RADIUS_MIN * math.pi / 2):
            (x2, y2, theta2, v2) = util.turn_Right((x, y, theta, v), direction,
                                                   d, util.ROBOT_RADIUS_MIN)
            draw_point(x2, y2, theta2, 'green')
            d += 10

    elif action in ("L_f", "L_b"):  # turning left
        d = 0
        while (d <= util.ROBOT_RADIUS_MIN * math.pi / 2):
            (x2, y2, theta2, v2) = util.turn_Left((x, y, theta, v), direction,
                                                  d, util.ROBOT_RADIUS_MIN)
            draw_point(x2, y2, theta2, 'green')
            d += 10

    elif action in ("F", "B"):
        d = 0
        while (d <= util.CELL_SIZE):
            (x2, y2, theta2, v2) = util.go_Straight((x, y, theta, v),
                                                    direction, d)
            draw_point(x2, y2, theta2, 'green')
            d += 10

    elif action in ("F3"):
        d = 0
        while (d <= 3 * util.CELL_SIZE):
            (x2, y2, theta2, v2) = util.go_Straight((x, y, theta, v),
                                                    direction, d)
            draw_point(x2, y2, theta2, 'green')
            d += 10

    elif action == "SL_f":
        d = 0
        while (d <= 20.5 / 70.0 * util.ROBOT_RADIUS_MIN):
            (x2, y2, theta2, v2) = util.go_Straight((x, y, theta, v),
                                                    direction, d)
            draw_point(x2, y2, theta2, 'green')
            d += 10
        d = 0
        (x_temp, y_temp, theta_temp, v_temp) = util.go_Straight(
            (x, y, theta, v), direction, 20.5 / 70.0 * util.ROBOT_RADIUS_MIN)
        while (d <= util.ROBOT_RADIUS_2 * math.pi / 4):
            (x2, y2, theta2, v2) = util.turn_Left(
                (x_temp, y_temp, theta_temp, v_temp), direction, d,
                util.ROBOT_RADIUS_2)
            draw_point(x2, y2, theta2, 'green')
            d += 10

    elif action == "SR_f":
        d = 0
        while (d <= 20.5 / 70.0 * util.ROBOT_RADIUS_MIN):
            (x2, y2, theta2, v2) = util.go_Straight((x, y, theta, v),
                                                    direction, d)
            draw_point(x2, y2, theta2, 'green')
            d += 10
        d = 0
        (x_temp, y_temp, theta_temp, v_temp) = util.go_Straight(
            (x, y, theta, v), direction, 20.5 / 70.0 * util.ROBOT_RADIUS_MIN)
        while (d <= util.ROBOT_RADIUS_2 * math.pi / 4):
            (x2, y2, theta2, v2) = util.turn_Right(
                (x_temp, y_temp, theta_temp, v_temp), direction, d,
                util.ROBOT_RADIUS_2)
            draw_point(x2, y2, theta2, 'green')
            d += 10

    elif action == "LS_f":
        d = 0
        while (d <= util.ROBOT_RADIUS_2 * math.pi / 4):
            (x2, y2, theta2, v2) = util.turn_Left((x, y, theta, v), direction,
                                                  d, util.ROBOT_RADIUS_2)
            draw_point(x2, y2, theta2, 'green')
            d += 10
        d = 0
        (x_temp, y_temp, theta_temp, v_temp) = util.turn_Left(
            (x, y, theta, v), direction, util.ROBOT_RADIUS_2 * math.pi / 4,
            util.ROBOT_RADIUS_2)
        while (d <= 20.5 / 70.0 * util.ROBOT_RADIUS_MIN):
            (x2, y2, theta2, v2) = util.go_Straight(
                (x_temp, y_temp, theta_temp, v_temp), direction, d)
            draw_point(x2, y2, theta2, 'green')
            d += 10

    elif action == "RS_f":
        d = 0
        while (d <= util.ROBOT_RADIUS_2 * math.pi / 4):
            (x2, y2, theta2, v2) = util.turn_Right((x, y, theta, v), direction,
                                                   d, util.ROBOT_RADIUS_2)
            draw_point(x2, y2, theta2, 'green')
            d += 10
        d = 0
        (x_temp, y_temp, theta_temp, v_temp) = util.turn_Right(
            (x, y, theta, v), direction, util.ROBOT_RADIUS_2 * math.pi / 4,
            util.ROBOT_RADIUS_2)
        while (d <= 20.5 / 70.0 * util.ROBOT_RADIUS_MIN):
            (x2, y2, theta2, v2) = util.go_Straight(
                (x_temp, y_temp, theta_temp, v_temp), direction, d)
            draw_point(x2, y2, theta2, 'green')
            d += 10

    elif action == "F_diag":
        d = 0
        while (d <= util.CELL_SIZE * math.sqrt(2)):
            (x2, y2, theta2, v2) = util.go_Straight((x, y, theta, v),
                                                    direction, d)
            draw_point(x2, y2, theta2, 'green')
            d += 10

    elif action == "F_diag3":
        d = 0
        while (d <= 3 * util.CELL_SIZE * math.sqrt(2)):
            (x2, y2, theta2, v2) = util.go_Straight((x, y, theta, v),
                                                    direction, d)
            draw_point(x2, y2, theta2, 'green')
            d += 10

    elif action == "B_diag":
        d = 0
        while (d <= util.CELL_SIZE * math.sqrt(2)):
            (x2, y2, theta2, v2) = util.go_Straight((x, y, theta, v),
                                                    direction, d)
            draw_point(x2, y2, theta2, 'green')
            d += 10
예제 #6
0
def odometry():
    """
    Draw car, and update position
    """
    global CAR, ROBOT_X, ROBOT_Y, ROBOT_TH, ROBOT_D
    v = 0
    t1 = 0
    looptime = 0
    while (True):
        loopstart = time.time()
        action = ROBOT_ACTION
        d = ROBOT_SPEED * (looptime)
        print d, ROBOT_SPEED, "steer", ROBOT_STEER
        ROBOT_D += d
        if action in ("R_f", "L_f", "F", "F3", "SR_f", "SL_f", "LS_f", "RS_f",
                      "F_diag", "F_diag3"):
            direction = "f"
        else:
            direction = "b"

        if action in ("R_f", "R_b"):  # turning right
            (ROBOT_X, ROBOT_Y, ROBOT_TH, v2) = util.turn_Right(
                (ROBOT_X, ROBOT_Y, ROBOT_TH, v), direction, d,
                util.ROBOT_RADIUS_MIN)

        elif action in ("L_f", "L_b"):  # turning left
            (ROBOT_X, ROBOT_Y, ROBOT_TH, v2) = util.turn_Left(
                (ROBOT_X, ROBOT_Y, ROBOT_TH, v), direction, d,
                util.ROBOT_RADIUS_MIN)

        elif action in ("F", "B"):
            (ROBOT_X, ROBOT_Y, ROBOT_TH, v2) = util.go_Straight(
                (ROBOT_X, ROBOT_Y, ROBOT_TH, v), direction, d)

        elif action in ("F3"):
            (ROBOT_X, ROBOT_Y, ROBOT_TH, v2) = util.go_Straight(
                (ROBOT_X, ROBOT_Y, ROBOT_TH, v), direction, d)

        elif action in ("SL_f", "LS_f"):
            if ROBOT_STEER == 0:
                (ROBOT_X, ROBOT_Y, ROBOT_TH, v2) = util.go_Straight(
                    (ROBOT_X, ROBOT_Y, ROBOT_TH, v), direction, d)
            elif ROBOT_STEER == -70:
                (ROBOT_X, ROBOT_Y, ROBOT_TH, v2) = util.turn_Left(
                    (ROBOT_X, ROBOT_Y, ROBOT_TH, v), direction, d,
                    util.ROBOT_RADIUS_2)

        elif action == ("SR_f", "RS_f"):
            if ROBOT_STEER == 0:
                (ROBOT_X, ROBOT_Y, ROBOT_TH, v2) = util.go_Straight(
                    (ROBOT_X, ROBOT_Y, ROBOT_TH, v), direction, d)
            elif ROBOT_STEER == 70:
                (ROBOT_X, ROBOT_Y, ROBOT_TH, v2) = util.turn_Right(
                    (ROBOT_X, ROBOT_Y, ROBOT_TH, v), direction, d,
                    util.ROBOT_RADIUS_2)

        elif action == "F_diag":
            (ROBOT_X, ROBOT_Y, ROBOT_TH, v2) = util.go_Straight(
                (ROBOT_X, ROBOT_Y, ROBOT_TH, v), direction, d)

        elif action == "F_diag3":
            (ROBOT_X, ROBOT_Y, ROBOT_TH, v2) = util.go_Straight(
                (ROBOT_X, ROBOT_Y, ROBOT_TH, v), direction, d)

        elif action == "B_diag":
            (ROBOT_X, ROBOT_Y, ROBOT_TH, v2) = util.go_Straight(
                (ROBOT_X, ROBOT_Y, theta, v), direction, d)

        else:
            return
        #time.sleep(0.1)
        if (CAR != None):
            graphics.canvas.delete(CAR[0])
            graphics.canvas.delete(CAR[1])
        CAR = graphics.draw_car(ROBOT_X, ROBOT_Y, ROBOT_TH)
        graphics.canvas.update_idletasks()
        looptime = time.time() - loopstart

        if (not thread_control.isAlive()):
            print odomreturning
            return