def test_grid_boat_easy(grid_data):
text_grid = grid_data.grid.strip().split('\n')
grid = Grid("", text_grid)
for i in range(len(text_grid[0])):
for j in range(len(text_grid)):
if text_grid[j][i] == 'B':
assert grid.boat == Node(True, i, j) or grid.boat == Node(True, j, i)
return
def dijkstra(self, start_position, goal_position):
"""
Plans a path using the Dijkstra algorithm.
:param start_position: position where the planning stars as a tuple (x, y).
:type start_position: tuple.
:param goal_position: goal position of the planning as a tuple (x, y).
:type goal_position: tuple.
:return: the path as a sequence of positions and the path cost.
:rtype: list of tuples and float.
"""
# Todo: implement the Dijkstra algorithm
# The first return is the path as sequence of tuples (as returned by the method construct_path())
# The second return is the cost of the path
self.node_grid.reset() #seta todas as distâncias para infinito
i = start_position[0]
j = start_position[1]
self.node_grid.grid[i, j].f = 0
pq = []
heapq.heappush(
pq, (self.node_grid.grid[i, j].f, self.node_grid.grid[i, j]))
while True:
davez = Node(300, 300)
while len(pq) != 0:
atual = pq.pop(0)
if atual[1].closed == False:
davez = atual[1]
break
if davez.i == 300:
break
self.node_grid.grid[davez.i, davez.j].closed = True
t = davez.get_position()
p = self.node_grid.get_successors(t[0], t[1])
for item in p:
n = (davez.i, davez.j)
m = (self.node_grid.grid[item[0], item[1]].i,
self.node_grid.grid[item[0], item[1]].j)
dist = self.cost_map.get_edge_cost(n, m)
h = item[0]
k = item[1]
if self.node_grid.grid[
h, k].f > davez.f + dist and self.node_grid.grid[
h, k].closed == False:
self.node_grid.grid[h, k].parent = davez
self.node_grid.grid[h, k].f = davez.f + dist
heapq.heappush(pq, (self.node_grid.grid[h, k].f,
self.node_grid.grid[h, k]))
pt = self.construct_path(self.node_grid.grid[goal_position[0],
goal_position[1]])
ct = self.node_grid.grid[goal_position[0], goal_position[1]].f
return pt, ct
def test_grid_node_easy(grid_data):
text_grid = grid_data.grid.strip().split('\n')
grid = Grid("", text_grid)
for i in range(len(text_grid[0])):
for j in range(len(text_grid)):
navigable = text_grid[j][i] == '.'
try:
node = grid.map[i][j]
except IndexError:
node = grid.map[j][i]
assert node == Node(navigable, i, j) or node == Node(navigable, j, i)
def test_grid_node_hard(grid_data):
text_grid = grid_data.grid.strip().split('\n')
grid = Grid("", text_grid)
for i in range(len(text_grid[0])):
for j in range(len(text_grid)):
navigable = text_grid[j][i] == '.'
assert grid.map[i][j] == Node(navigable, i, j)
def a_star(self, start_position, goal_position):
"""
Plans a path using A*.
:param start_position: position where the planning stars as a tuple (x, y).
:type start_position: tuple.
:param goal_position: goal position of the planning as a tuple (x, y).
:type goal_position: tuple.
:return: the path as a sequence of positions and the path cost.
:rtype: list of tuples and float.
"""
# Todo: implement the A* algorithm
# The first return is the path as sequence of tuples (as returned by the method construct_path())
# The second return is the cost of the path
pq = []
goal = self.node_grid.grid[goal_position[0], goal_position[1]]
start = self.node_grid.grid[start_position[0], start_position[1]]
start.g = 0
start.f = Node.distance_to(start, goal_position[0], goal_position[1])
heapq.heappush(pq, (start.f, start))
while not (len(pq) == 0):
f, node = heapq.heappop(pq)
node.closed = True
if (node.i == goal_position[0]) and (node.j == goal_position[1]):
goal.g = goal.parent.g + self.cost_map.get_edge_cost(
goal, goal.parent)
goal.f = goal.g + 0
break
for sucessor in self.node_grid.get_successors(node.i, node.j):
sucessor = self.node_grid.grid[sucessor]
if (sucessor.f >
node.g + self.cost_map.get_edge_cost(node, sucessor) +
Node.distance_to(sucessor, goal_position[0],
goal_position[1])):
sucessor.g = node.g + self.cost_map.get_edge_cost(
node, sucessor)
sucessor.f = sucessor.g + Node.distance_to(
sucessor, goal_position[0], goal_position[1])
sucessor.parent = node
heapq.heappush(pq, (sucessor.f, sucessor))
path = self.construct_path(goal)
cost = goal.f
self.node_grid.reset()
return path, cost
def optimalHeuristic(node, closed, goal):
D = 3
nX, nY = node.position
gX, gY = goal
dX = abs(nX - gX)
dY = abs(nY - gY)
for n in getNeighborsA(node, closed):
if n not in closed:
mX, mY = n
if matrix[mX][mY] != "0":
suc = Node(node, n)
nodeCost = getCostA(matrix, node, suc)
def A(matrix, start, goal, weight, H):
fringe = []
closed = set()
cost = 0
nodeStart = Node(start, start)
nodeStart.W = weight
fringe.append(nodeStart)
while fringe:
fringe.sort()
node = fringe.pop(0)
if node not in closed:
closed.add(node.position)
if node.position == goal:
path = []
while node != nodeStart:
mem = sys.getsizeof(fringe) + sys.getsizeof(closed)
path.append(node.position)
mX, mY = node.position
node = node.parentNode
cost += getCostA(matrix, node, Node(0, (mX, mY)))
return path, cost, mem, len(closed)
n = getNeighborsA(node, closed)
for x in n:
if x not in closed:
mX, mY = x
if matrix[mX][mY] != "0":
suc = Node(node, x)
suc.W = weight
nodeCost = getCostA(matrix, node, suc)
if H == "D":
suc.H = optimalHeuristic(suc, closed, goal)
elif H == "N":
suc.H = math.trunc(distance((suc.position, goal)))
if suc not in fringe:
suc.G = sys.maxsize
if node.G + nodeCost < suc.G:
suc.G = node.G + nodeCost
suc.parentNode = node
suc.cost = suc.G + suc.W * suc.H
if suc in fringe:
fringe.remove(suc)
fringe.append(suc)
return None
def test_grid_boat(self):
actual = self.grid.boat
expected = Node(True, 3, 1)
msg = "Expected boat in (3, 1), got {}".format(actual)
self.assertEqual(actual, expected, msg)
def test_grid_node(self):
actual = self.grid.map[6][4]
expected = Node(False, 6, 4)
msg = "Expected non-navigable node (4, 6), got {}".format(actual)
self.assertEqual(actual, expected, msg)
def setUp(self):
self.n1 = Node(True, 1, 2)
self.n2 = Node(True, 2, 2)
self.n3 = Node(True, 2, 2)
this_p[2] + k
]))
o_pose = robot.GetTransform()
startconfig = [o_pose[0][3], o_pose[1][3], 0]
step_len = 0.1
x_grid_num = int(7.4 / step_len)
y_grid_num = int(3.4 / step_len)
theta_num = 4
grid = []
for i in range(x_grid_num):
line = []
for j in range(y_grid_num):
line.append([
Node(-3.7 + i * step_len, -1.7 + j * step_len,
pi - 2 * pi / theta_num * k, (i, j, k), (-1, -1, -1),
0) for k in range(theta_num)
])
grid.append(line)
start_p = [
int(round((startconfig[0] + 3.7) / step_len)),
int(round((startconfig[1] + 1.7) / step_len)), theta_num / 2
]
end_p = [
int(round((goalconfig[0] + 3.7) / step_len)),
int(round((goalconfig[1] + 1.7) / step_len)), 3 * theta_num / 4
]
#### Implement your algorithm to compute a path for the robot's base starting from the current configuration of the robot and ending at goalconfig. The robot's base DOF have already been set as active. It may be easier to implement this as a function in a separate file and call it here.
q = PriorityQueue()
grid[start_p[0]][start_p[1]][start_p[2]].parentid = (-2, -2, -2)
def test_fcost(hcost, gcost, fcost):
n = Node(False, 0, 0)
n.set_hcost(hcost)
n.set_gcost(gcost)
assert n.fcost() == fcost
from grid import Node
@pytest.mark.parametrize("hcost, gcost, fcost", [
(3.0, 2.0, 5.0),
])
def test_fcost(hcost, gcost, fcost):
n = Node(False, 0, 0)
n.set_hcost(hcost)
n.set_gcost(gcost)
assert n.fcost() == fcost
@pytest.mark.parametrize("node, parent", [
(Node(True, 1, 2), Node(True, 3, 4)),
])
def test_set_parent(node, parent):
node.set_parent(parent)
assert id(node.parent) == id(parent)
@pytest.mark.parametrize("n1, n2, distance", [
(Node(True, 0, 0), Node(False, 0, 1), 10),
(Node(True, 1, 0), Node(False, 0, 0), 10),
(Node(True, 0, 0), Node(False, 1, 1), 14),
(Node(True, 1, 0), Node(False, 0, 1), 14),
(Node(True, 8, 4), Node(False, 1, 10), 94),
])
def test_distance(n1, n2, distance):
assert n1.distance(n2) == distance