def run(obstacles, start, goal, max_size, plotter):
step_size = 50
final_pos = np.array(goal[:2])
# Pre-compute for generating new random points in Q_free
minp, maxp = obstacles.vertices.min(0), obstacles.vertices.max(0)
span, offset = (maxp-minp)*1.1, minp-(maxp-minp)*0.05
def gen_valid_rand(valid_function):
""" Generates a valid q_rand in Q_free given a validity function """
tmp = np.random.random(2) * span + offset
while not valid_function(*tmp):
tmp = np.random.random(2) * span + offset
return tmp
KD = KDTree(start[:2], 0)
circ1 = plotter.draw_circle(start, 1, time=1, zorder=5)
obstacles = Obstacles(obstacles.to_polygons())
trials = 0
while KD.length < max_size:
trials += 1
circ1.remove()
# Select a random point q_rand \in Q_free
q_rand = gen_valid_rand(obstacles.point_is_valid) if np.random.randint(0, 100)>5 else final_pos
circ1 = plotter.draw_circle(q_rand, 5, time=0.01, zorder=5)
# Find the nearest node and distance to it
q_near, dist = KD.nearestNode(q_rand, return_node=True)
# Generate the next node in the direction of q_rand
if dist < 0.5: continue
if dist < step_size:
if trials < 10: continue
q_next = tuple(q_rand)
else:
q_next = gen_next(tuple(q_near.node), q_rand, step_size)
if not obstacles.point_is_valid(*q_next): continue
dist = math.hypot(q_next[0]-q_near[0], q_next[1]-q_near[1])
# Check validity and update tree
for i in range(10):
alpha_new = np.random.random() * 2*math.pi #( + q_near.alpha) - math.pi
collides = check_collision(obstacles, (*q_near.node, q_near.alpha), (*q_next, alpha_new), dist)
if not collides: break
else: continue
KD.addNode(q_next, alpha_new)
plot_steps((*q_near.node, q_near.alpha), (*q_next, alpha_new), dist, plotter)
goal_distance = math.hypot(q_next[0]-goal[0], q_next[1]-goal[1])
collides = check_collision(obstacles, (*q_next, alpha_new), goal, goal_distance)
if not collides:
plot_steps((*q_next, alpha_new), goal, goal_distance, plotter)
plotter.draw_rectangle(gen_rect_pts(*goal), facecolor='red', edgecolor='k')
break
trials = 0
print("n =", KD.length)
def main():
XDIM = 500
YDIM = 500
WINSIZE = (XDIM, YDIM)
MAX_NUM_NODES = 800
MIN_NUM_NODES = 400
pygame.init()
screen = pygame.display.set_mode(WINSIZE)
pygame.display.set_caption('RRT* Path Planning')
screen.fill(WHITE)
running = True
# Obstacles
obs = Obstacles(screen, GRAY)
obs.make_circle(150, 150, 50)
obs.make_rect(250, 100, 50, 300)
obs.draw()
obs_resolution = 5
start_point = (50, 50)
goal_point = (400, 400)
goal_tolerance = 20
rrt_star = RRTStar(start_point, goal_point, MAX_NUM_NODES, MIN_NUM_NODES,
goal_tolerance, 0, 30, screen, obs, obs_resolution)
path = rrt_star.planning()
print "Final Path: "
print path
while running:
for event in pygame.event.get():
if event.type == pygame.QUIT:
running = False
def __init__(self, track, riders, players):
self.track = track
self.riders = riders
self.obstacles = Obstacles(riders)
self.players = players
self.arrivals = []
self.checkArrivals()
def create_obstacle(settings, obs, grounds, screen):
random_number2 = randint(settings.ob_number - 2, settings.ob_number)
for x in range(1, random_number2):
ob = Obstacles(settings, screen, grounds)
random_number = randint(0, len(ob.images) - 1)
ob.index = random_number
ob.pick_obstacle(grounds)
if x != 1:
if x != random_number2:
random_offset = randint(-20, 20)
ob.x += settings.previous_position + random_offset
settings.previous_position += ob.rect.width
obs.append(ob)
settings.previous_position = 0
def testObstacles(self):
riders = [
RiderToken(0, 0),
RiderToken(1, 0),
RiderToken(2, 1),
RiderToken(3, 0),
RiderToken(3, 1)
]
obstacles = Obstacles(riders)
assert_equals([(0, 0), (1, 1), (2, 0), (3, 2), (4, 0)],
findPath(obstacles, (0, 0), (4, 0)))
def main():
XDIM = 500
YDIM = 500
WINSIZE = [XDIM, YDIM]
EPSILON = 7.0
MAX_NUM_NODES = 1000
MIN_NUM_NODES = 500
pygame.init()
screen = pygame.display.set_mode(WINSIZE)
pygame.display.set_caption('RRT* Dual Tree Path Planning')
screen.fill(WHITE)
# Obstacles
obs = Obstacles(screen, BLACK)
obs.make_circle(150, 150, 50)
obs.make_rect(250, 100, 50, 300)
obs.draw()
obs_resolution = 5
start_point = [50, 50]
goal_point = [400, 400]
goal_tolerance = 20
rrt_star = RRTStarDualTree(start_point, goal_point, MAX_NUM_NODES,
MIN_NUM_NODES, goal_tolerance, 0, 30, screen,
obs, obs_resolution)
path = rrt_star.planning()
pause = True
# for e in pygame.event.get():
# if e.type == QUIT or (e.type == KEYUP and e.key == K_ESCAPE):
# sys.exit("Leaving because you requested it.")
# pygame.display.update()
while pause:
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.quit()
quit()
def dwa_wrapper(final_list):
odom_dec = {}
q = Quaternion(final_list[0].pose.pose.orientation.w,
final_list[0].pose.pose.orientation.x,
final_list[0].pose.pose.orientation.y,
final_list[0].pose.pose.orientation.z)
e = q.to_euler(degrees=False)
odom_dec["x"] = final_list[0].pose.pose.position.x
odom_dec["y"] = final_list[0].pose.pose.position.y
odom_dec["theta"] = e[2]
odom_dec["u"] = final_list[0].twist.twist.linear.x
odom_dec["omega"] = final_list[0].twist.twist.angular.z
cnfg = Config(odom_dec, final_list[1])
obs = Obstacles(final_list[2].ranges, cnfg)
v_list, w_list, cost_list = DWA(cnfg, obs)
return v_list, w_list, cost_list, final_list[3]
def main():
XDIM = 500
YDIM = 500
WINSIZE = (XDIM, YDIM)
MAX_NUM_NODES = 800
MIN_NUM_NODES = 400
pygame.init()
# set_mode(size=(0, 0), flags=0, depth=0, display=0, vsync=0)
screen = pygame.display.set_mode(WINSIZE, pygame.RESIZABLE, 32)
pygame.display.set_caption('RRT*-Smart Path Planning')
screen.fill(WHITE)
# Obstacles
obs = Obstacles(screen, BLACK)
obs.make_circle(150, 150, 50)
obs.make_rect(250, 100, 50, 300)
obs.draw()
start_point = (50, 50)
goal_point = (450, 450)
goal_tolerance = 20
obs_resolution = 5
rrt_star_smart = RRTStarSmart(start_point, goal_point, MAX_NUM_NODES,
MIN_NUM_NODES, goal_tolerance, 0, 30, screen,
obs, obs_resolution)
path = rrt_star_smart.planning()
pause = True
# for e in pygame.event.get():
# if e.type == QUIT or (e.type == KEYUP and e.key == K_ESCAPE):
# sys.exit("Leaving because you requested it.")
# pygame.display.update()
while pause:
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.quit()
quit()
def __init__(self):
self.cfg = config.Config(file('config.ini'))
self.game_start_time = time.time()
self.game_speed = self.cfg.init_game_speed
self.play_time = 0
self.game_score = 0
self.player_name = ''
self.fps = self.cfg.fps
pygame.init()
try:
if sys.argv[1] == 'fullscreen':
info = pygame.display.Info()
self.window_size = (info.current_w, info.current_h)
pygame.display.set_mode(self.window_size, pygame.FULLSCREEN)
except:
self.window_size = (640, 480)
self.screen = pygame.display.set_mode(self.window_size)
self.clock = pygame.time.Clock()
self.obstacles = Obstacles(self.screen, self.window_size)
self.obstacles.gen_obstacles(1)
self.player = Player(self.screen, self.window_size)
self.bg = Background(self.screen)
def main():
#changing the height might mess up the sonar, let me know if it does
width, height = 1280, 800
#create game screen & physics
pygame.init()
running = True
screen = pygame.display.set_mode((width, height))
clock = pygame.time.Clock()
font = pygame.font.SysFont("Arial", 16)
space = pymunk.Space()
space.gravity = 0, 0
#create player (x pos, y pos, radius)
player = Player(space, screen, 400, 400, 30)
#collision handler callback
def handle(arbiter, space, data):
player.reset()
player.collided = True
return True
handler = space.add_collision_handler(1, 2)
handler.begin = handle
#dont omit
draw_options = pymunk.pygame_util.DrawOptions(screen)
#create obstacles
Obstacles.addWalls(space)
Obstacles.addObstacles(space)
#create nn
nn = NN()
while running:
for event in pygame.event.get():
if event.type == QUIT or \
event.type == KEYDOWN and (event.key in [K_ESCAPE, K_q]):
running = False
screen.fill(pygame.color.THECOLORS["black"])
#draw screen, update physics objects
#physics objects must be updated & drawn first so that sonar dots can detect red color
space.debug_draw(draw_options)
pygame.display.flip()
dist = player.drawSonar() #array of distance values, [left up right]
screen.fill(pygame.color.THECOLORS["black"])
#choose action
action = nn.getAction()
#do action
player.rotate(0.01)
#get reward
reward = player.getReward()
#update screen
space.debug_draw(draw_options)
pygame.display.flip()
#get new dist
dist = player.drawSonar()
#train
nn.getHighestQPrime()
nn.train(123123)
fps = 60
dt = 1. / fps
space.step(dt)
clock.tick(fps)
input()
class Game:
def __init__(self):
self.cfg = config.Config(file('config.ini'))
self.game_start_time = time.time()
self.game_speed = self.cfg.init_game_speed
self.play_time = 0
self.game_score = 0
self.player_name = ''
self.fps = self.cfg.fps
pygame.init()
try:
if sys.argv[1] == 'fullscreen':
info = pygame.display.Info()
self.window_size = (info.current_w, info.current_h)
pygame.display.set_mode(self.window_size, pygame.FULLSCREEN)
except:
self.window_size = (640, 480)
self.screen = pygame.display.set_mode(self.window_size)
self.clock = pygame.time.Clock()
self.obstacles = Obstacles(self.screen, self.window_size)
self.obstacles.gen_obstacles(1)
self.player = Player(self.screen, self.window_size)
self.bg = Background(self.screen)
def display_text(self, text, x=None, y=None, font_size=40):
myfont = pygame.font.SysFont("Arial Bold", font_size)
label = myfont.render(text, 1, (255, 255, 255))
if not x:
x = self.window_size[0] / 2 - label.get_width() / 2
if not y:
y = self.window_size[1] / 3
self.screen.blit(label, (x, y))
return label
def game_over(self):
self.display_text('GAME OVER', None, 50)
self.display_text("your score is %s" % self.game_score, None, 80)
def game_score_display(self):
self.play_time = time.time() - self.game_start_time
self.display_text("Score: %s" % str(self.game_score), 10, 10)
def game_score_save(self, key_index=None):
if len(self.player_name) < 3:
if key_index != None and key_index > 96 and key_index < 123:
self.player_name += chr(key_index)
self.display_text('Type your initials: %s' % self.player_name, None, 120, 30)
elif len(self.player_name) == 3:
writer = csv.writer(open('scores.txt', 'ab'))
writer.writerow([self.player_name, self.game_score])
self.player_name += '-1'
elif int(self.player_name[-1:]) == 1:
self.player_name = 'abc2'
reader = csv.reader(open('scores.txt', 'rb'))
scores = []
for row in reader:
scores.append((row[0], row[1]))
scores.sort(None, operator.itemgetter(1), True)
writer = csv.writer(open('scores.txt', 'wb'))
writer.writerows(scores[:5])
else:
self.game_score_board()
def game_score_board(self):
reader = csv.reader(open('scores.txt', 'rb'))
y = 120
self.display_text("Top scores", None, y, 30)
y = 150
for row in reader:
self.display_text('%s %s' % (row[0].upper(), row[1]), None, y, 30)
y += 30
def game_speedup(self, speedup=1):
self.game_speed += speedup
def draw(self, hide_scores=False):
self.bg.draw()
self.obstacles.draw()
self.player.draw()
if not hide_scores:
self.game_score_display()
def main(self):
done = False
stop = False
gameover = False
speedup_counter = self.fps
while done == False:
if not gameover:
if self.player.hit_obstacle(self.obstacles.collection):
gameover = True
key_index = None
for event in pygame.event.get():
if event.type == pygame.QUIT:
done = True
if event.type == pygame.KEYUP:
if event.key == pygame.K_ESCAPE:
done = True
if gameover:
key_index = event.key
self.player.keyboard_move(event)
self.player.mouse_move()
self.obstacles.up(self.game_speed)
if speedup_counter > 0:
speedup_counter -= 1
else:
if int(self.play_time) % 5 == 0 and self.play_time != 0:
self.game_speedup()
if len(self.obstacles.collection) < self.cfg.max_obstacles:
self.obstacles.one_more()
speedup_counter = self.fps
if not stop:
self.clock.tick(self.fps)
self.draw(gameover)
if gameover:
self.game_over()
self.game_score_save(key_index)
stop = True
else:
self.game_score += 1
pygame.display.flip()
self.clock.tick(self.fps)
return False
def gen_rand(low, high, size):
arrs = []
for i in range(len(low)):
span = high[i]-low[i]
arrs.append((np.random.random(size)*span*1.1 + low[i]-span*0.05).reshape(-1,1))
return np.concatenate(arrs, 1)
if __name__ == '__main__':
obstacle_path, goal_path = "../hw3/world_obstacles.txt", "../hw3/goal.txt"
# obstacle_path, goal_path = "world_obstacles.txt", "start_goal.txt"
path = get_obstacle_course(obstacle_path)
start, goal = get_start_and_goal(goal_path)
obstacles = Obstacles(path.to_polygons())
shapes = [Polygon(polyg) for polyg in path.to_polygons()]
num_tests = 10000
""" Unit test for line in obstacle """
minp, maxp = path.vertices.min(0), path.vertices.max(0)
step_size = 50
starts = gen_rand(minp, maxp, num_tests)
rands = gen_rand(minp, maxp, num_tests)
# starts, rands = starts.astype(int), rands.astype(int)
dirs = starts - rands
ends = starts + dirs / np.hypot(dirs[:,0], dirs[:,1])[:,None] * step_size
paths = [Path([starts[i], ends[i]], [Path.MOVETO, Path.LINETO]) for i in range(num_tests)]
return False
def gen_rand(low, high, size):
arrs = []
for i in range(len(low)):
span = high[i]-low[i]
arrs.append((np.random.random(size)*span*1.1 + low[i]-span*0.05).reshape(-1,1))
return np.concatenate(arrs, 1)
if __name__ == '__main__':
obstacle_path, goal_path = "../hw3/world_obstacles.txt", "../hw3/goal.txt"
# obstacle_path, goal_path = "world_obstacles.txt", "start_goal.txt"
path = get_obstacle_course(obstacle_path)
start, goal = get_start_and_goal(goal_path)
obstacles = Obstacles(path.to_polygons())
shapes = [Polygon(polyg) for polyg in path.to_polygons()]
num_tests = 10000
""" Unit test for point in obstacle """
minp, maxp = path.vertices.min(0), path.vertices.max(0)
points = gen_rand(minp, maxp, num_tests)
# points = points.astype(int)
my_ans = [not obstacles.point_is_valid(*points[i]) for i in range(num_tests)]
ex_ans = [shapely_contains(points[i]) for i in range(num_tests)]
correct_positives, correct_negatives = [], []
false_positives, false_negatives = [], []
for i in range(num_tests):
"""
Main Program File to start 2D Drone Simulation
"""
import pygame
from environment import Environment
from obstacles import Obstacles
from drone import Drone
if __name__ == "__main__":
# Initialize Environment
env = Environment()
# Initialize Obstacles
obstacles = Obstacles(env)
# Initialize Drone
drone = Drone(env)
# Main loop
while env.running:
# Update all environment variables first (dt)
env.update()
# for loop through the event queue
for event in pygame.event.get():
# Get Keys which are held down (easier for drone control)
pressed = pygame.key.get_pressed()
drone.check_user_input(pressed)
def testMove(self):
rider = Rider(rouleurShade, "green")
self.logger.logMove(rider, (0, 0), (3, 1), Obstacles([]))
self.display.displayRiders([rider])
self.animate()
def run(obstacles, start, goal, step_size, max_size, plotter):
circ_rad = min(step_size / 5, 5)
final_pos = np.array(goal[:2])
# Pre-compute for generating new random points in Q_free
minp, maxp = obstacles.vertices.min(0), obstacles.vertices.max(0)
span, offset = (maxp - minp) * 1.1, minp - (maxp - minp) * 0.05
def gen_valid_rand(valid_function):
""" Generates a valid q_rand in Q_free given a validity function """
tmp = np.random.random(2) * span + offset
while not valid_function(*tmp):
tmp = np.random.random(2) * span + offset
return tmp
KD = KDTree(start)
RRT = PathTree(start)
circ1 = plotter.draw_circle(start, 1, time=1, zorder=5)
obstacles = Obstacles(obstacles.to_polygons())
trials = 0
while KD.length < max_size:
trials += 1
circ1.remove()
# Select a random point q_rand \in Q_free
q_rand = gen_valid_rand(obstacles.point_is_valid) if np.random.randint(
0, 100) > 5 else final_pos
circ1 = plotter.draw_circle(q_rand, 5, time=0.01, zorder=5)
# Find the nearest node and distance to it
q_near, dist = KD.nearestNode(q_rand)
# Generate the next node in the direction of q_rand
if dist < step_size:
if trials < 10: continue # Prevents step_size too big bug
q_next = tuple(q_rand)
else:
q_next = gen_next(q_near, q_rand, step_size)
if not obstacles.point_is_valid(*q_next): continue
# Check validity and update tree
if obstacles.check_collisions((q_near, q_next)): continue
KD.addNode(q_next)
RRT.addPath(q_near, q_next)
plotter.draw_line(q_near, q_next, color='k', zorder=1, update=False)
plotter.draw_circle(q_next,
circ_rad,
edgecolor='k',
facecolor='w',
zorder=2)
if not obstacles.check_collisions((q_next, goal)):
# IF there is a direct line to the goal, then TAKE IT
goal_distance = math.hypot(q_next[0] - goal[0],
q_next[1] - goal[1])
while goal_distance > 0:
q_new = gen_next(q_next, goal, min(goal_distance, step_size))
RRT.addPath(q_next, q_new)
plotter.draw_line(q_next,
q_new,
color='k',
zorder=1,
update=False)
plotter.draw_circle(q_new,
circ_rad,
edgecolor='k',
facecolor='w',
zorder=2)
q_next = q_new
goal_distance -= step_size
break
trials = 0
print("n =", KD.length)
cur = RRT[goal]
while cur.parent:
plotter.draw_line(cur, cur.parent, update=False, color='b', zorder=3)
plotter.draw_circle(cur,
circ_rad * 1.5,
update=False,
facecolor='xkcd:green',
edgecolor='k',
zorder=4)
cur = cur.parent
plotter.update()
plt.plot(points_x, points_y, color='red', linewidth=2)
plt.plot(points_x[0], points_y[0], color='blue', zorder=0)
plt.scatter(robot_pose.x, robot_pose.y, s=10**2, color='blue', zorder=1)
plt.scatter(8, 8, s=10**2, color='green', zorder=1)
if obstacle_pose is not (None):
plt.scatter(obstacle_pose.x,
obstacle_pose.y,
s=10**2,
color='red',
zorder=1)
plt.show()
# main function to intial RRT* algorithm
map_obstacles = Obstacles()
obstacles = []
obstacles.append(Polygon([(-1.1, 7.6), (1.1, 7.6), (1.1, -7.6), (-1.1, -7.6)]))
obstacles.append(Polygon([(-7.6, 1.1), (7.6, 1.1), (7.6, -1.1), (-7.6, -1.1)]))
obstacles.append(Polygon([(-10, 9.4), (-10, 10), (10, 10), (10, 9.4)]))
obstacles.append(Polygon([(-10, 10), (-10, -10), (-9.4, -10), (-9.4, 10)]))
obstacles.append(Polygon([(-10, -10), (10, -10), (10, -9.4), (-10, -9.4)]))
obstacles.append(Polygon([(9.4, -10), (10, -10), (10, 10), (9.4, 10)]))
map_obstacles.addNewObstacle(obstacles)
obstacles_list = map_obstacles.getObstacles()
cost_total = []
rrt = RRT(obstacles_list)
##################################################################################################################
def run(obstacles, start, goal, step_size, max_size, plotter):
circ_rad = min(step_size/5, 5)
final_pos = [np.array(goal), np.array(start)]
# Pre-compute for generating new random points in Q_free
minp, maxp = obstacles.vertices.min(0), obstacles.vertices.max(0)
span, offset = (maxp-minp)*1.1, minp-(maxp-minp)*0.05
def gen_valid_rand(valid_function):
""" Generates a valid q_rand in Q_free given a validity function """
tmp = np.random.random(2) * span + offset
while not valid_function(*tmp):
tmp = np.random.random(2) * span + offset
return tmp
KD = [KDTree(start), KDTree(goal)]
RRT = [PathTree(start), PathTree(goal)]
n = 1
rnd_display = False
obstacles = Obstacles(obstacles.to_polygons())
"""
• Expand tree T_1 randomly, add node q_new
• Expand T_2 towards q_new
• If tree T_2 connects to q_new, path formed else add a q_new for tree T_2
• Now expand T_1 to q_new in tree T_2
• Keep swapping T_1 and T_2 for expansion towards the
other tree until they meet
"""
trials = 0
q_new, last_expanded = None, -1
while KD[0].length + KD[1].length < 1000:
trials += 1
if rnd_display: circ1.remove(); rnd_display = False
n = 1 - n
# If the last expanded node was in the other tree, try expanding towards q_new
if last_expanded != n and q_new is not None:
q_near, dist = KD[n].nearestNode(q_new)
q_next = gen_next(q_near, q_new, step_size) if dist>step_size else q_new
# Expansion towards q_new is possible. Add to path and goal check
if obstacles.point_is_valid(*q_next) and \
not obstacles.check_collisions((q_near, q_next)):
RRT[n].addPath(q_near, q_next)
KD[n].addNode(q_next)
plotter.draw_circle(q_next, circ_rad, edgecolor='k', facecolor='w', zorder=1)
plotter.draw_line(q_near, q_next, color='kb'[n], zorder=1)
if q_next == q_new: break # Path found
q_new, last_expanded, trials = q_next, n, 0 # Update for next iteration
continue
# If last expanded node was not in the other tree or expansion to q_new not possible
# Try to expand to q_rand if possible
q_rand = gen_valid_rand(obstacles.point_is_valid) if np.random.randint(0,100)>5 else final_pos[n]
rnd_display, circ1 = True, plotter.draw_circle(q_rand, 5, zorder=5)
q_near, dist = KD[n].nearestNode(q_rand)
if dist < step_size:
if trials < 10: continue
q_next = tuple(q_rand)
else:
q_next = gen_next(q_near, q_rand, step_size)
if not obstacles.point_is_valid(*q_next): continue
if obstacles.check_collisions((q_near, q_next)): continue
KD[n].addNode(q_next)
RRT[n].addPath(q_near, q_next)
plotter.draw_line(q_near, q_next, color='kb'[n], zorder=1)
plotter.draw_circle(q_next, circ_rad, edgecolor='k', facecolor='w', zorder=1)
q_new, last_expanded, near_count = q_next, n, 0
print("n =", KD[0].length + KD[1].length, "(%d, %d)"%(KD[0].length, KD[1].length))
# Plot out goal path
cur = RRT[0][tuple(q_next)]
while cur.parent:
plotter.draw_line(cur, cur.parent, update=False, color='y', zorder=3)
plotter.draw_circle(cur, circ_rad*1.5, update=False, facecolor='xkcd:green', edgecolor='k', zorder=4)
cur = cur.parent
cur = RRT[1][tuple(q_next)]
while cur.parent:
plotter.draw_line(cur, cur.parent, update=False, color='y', zorder=3)
plotter.draw_circle(cur, circ_rad*1.5, update=False, facecolor='xkcd:green', edgecolor='k', zorder=4)
cur = cur.parent
plotter.update()
def testMoveOne(self):
obstacles = Obstacles([])
assert_equals([(0, 0), (1, 0)], findPath(obstacles, (0, 0), (1, 0)))
def testMoveStraight(self):
obstacles = Obstacles([])
assert_equals([(0, 0), (1, 0), (2, 0), (3, 0), (4, 0)],
findPath(obstacles, (0, 0), (4, 0)))
