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()
