예제 #1
0
파일: GUI.py 프로젝트: EVMakers/ballbot
def received_odometry(data):
    global Ballbot_x,Ballbot_y,Ballbot_theta,Ballbot_Tkobjects
    # Coordinate frame conversion from localization frame to planner frame
    Ballbot_x = (data.y + 3.658)
    Ballbot_y = (30.17 - data.x)
    Ballbot_theta = (data.theta - math.pi/2)%(2*math.pi)
    #print (Ballbot_x,Ballbot_y,Ballbot_theta)
    """
    delete old car
    """
    if(Ballbot_Tkobjects!=None):
        graphics.canvas.delete(Ballbot_Tkobjects[0])
        #graphics.canvas.delete(Ballbot_Tkobjects[1])
    """
    redraw car at new position
    """            
    #rospy.loginfo("drawing car at %f,%f,%f",Ballbot_x,Ballbot_y,Ballbot_theta)
    Ballbot_Tkobjects = graphics.draw_car(Ballbot_x*100.0 + 17.41*math.cos(Ballbot_theta) ,Ballbot_y*100.0+ 17.41*math.sin(Ballbot_theta),Ballbot_theta)
예제 #2
0
def received_odometry(data):
    global Ballbot_x, Ballbot_y, Ballbot_theta, Ballbot_Tkobjects
    # Coordinate frame conversion from localization frame to planner frame
    Ballbot_x = (data.y + 3.658)
    Ballbot_y = (30.17 - data.x)
    Ballbot_theta = (data.theta - math.pi / 2) % (2 * math.pi)
    #print (Ballbot_x,Ballbot_y,Ballbot_theta)
    """
    delete old car
    """
    if (Ballbot_Tkobjects != None):
        graphics.canvas.delete(Ballbot_Tkobjects[0])
        #graphics.canvas.delete(Ballbot_Tkobjects[1])
    """
    redraw car at new position
    """
    #rospy.loginfo("drawing car at %f,%f,%f",Ballbot_x,Ballbot_y,Ballbot_theta)
    Ballbot_Tkobjects = graphics.draw_car(
        Ballbot_x * 100.0 + 17.41 * math.cos(Ballbot_theta),
        Ballbot_y * 100.0 + 17.41 * math.sin(Ballbot_theta), Ballbot_theta)
예제 #3
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