Exemple #1
0
def step():
    global cur_x, cur_y, cur_dir, cur_lin, cur_ang, goal_x, goal_y

    plotEverything()

    mycloud = []
    for cloudPoint in cloud:
        if GLib.euclid(cloudPoint.x, cloudPoint.y, cur_x, cur_y) < CLEARANCE_MAX:
            mycloud.append(cloudPoint);

    (cur_ang, cur_lin) = calcDynamicWindow(
        cur_x,
        cur_y,
        cur_dir,
        cur_lin,
        cur_ang,
        mycloud,
        goal_x,
        goal_y,
        DT,
        plotter
        )

    kine = GLib.calcTrajectoryStepFromTime(
        cur_x,
        cur_y,
        cur_dir,
        cur_lin,
        cur_ang,
        DT
        )

    cur_x = kine[0]
    cur_y = kine[1]
    cur_dir = kine[2]

    plotter.plotArrow(cur_x, cur_y, cur_dir, "red")
    plotter.display()