def main(args):
# Astar Module
if args.model == "astar":
model = Astar()
# Reinforcement Learning module
elif args.model == "rl":
model = Rl()
# Dummy test
elif args.model == "dummy":
model = Dummy()
# Create an environment
env = gym.make('voilier-v2').unwrapped
state = env.reset()
# TODO Get range from argument
for step in range(0, 200, 1):
action = model.step(state)
state, _, _, _ = env.step(action)
env.render()
def __init__(self, n_features, n_actions):
self.n_features = n_features
self.n_actions = n_actions
# env, simulate observations
env = open('env_hole.pkl', 'rb')
self.observe_env = pickle.load(env)
env.close()
env = open('env_hole_vol.pkl', 'rb')
self.observe_vol = pickle.load(env)
env.close()
# current position
self.pos_x = None
self.pos_y = 1.0
self.pos_z = None
# 8 action dim
self.action_labels = [
'0', '45', '90', '135', '180', '225', '270', '315'
]
self.actor = Actor(self.n_features, self.n_actions, lr=0.004)
self.actor.load_trained_model("save/multiple/hole/save100.ckpt")
# fixme, first define critic before load : will report bug for not found in checkpoint
self.critic = Critic(self.n_features,
self.n_actions,
lr=0.003,
gamma=0.95)
# fixme, use trained model to predict
self.bin_graph = tf.Graph()
with self.bin_graph.as_default():
self.bin_classfic = BinSupervisor(366, 2)
# fixme, avoid obstacles, env defined in A star
self.astar = Astar()
def test_astar2(self):
nodes2 = {
"A": [("B", 2), ("E", 2)],
"B": [("A", 2), ("C", 2)],
"C": [("B", 2), ("D", 2)],
"D": [("C", 2), ("G", 2)],
"E": [("A", 2), ("J", 3)],
"F": [("B", 2), ("K", 2), ("G", 2)],
"G": [("F", 2), ("D", 2), ("L", 4), ("H", 3)],
"H": [("G", 3), ("I", 2)],
"I": [("H", 2), ("M", 3)],
"J": [("E", 3), ("O", 4)],
"K": [("F", 2), ("O", 2)],
"L": [("G", 4), ("M", 1), ("P", 5)],
"M": [("I", 3), ("L", 1)],
"O": [("K", 2), ("J", 4)],
"P": [("L", 5)]
}
astar2 = Astar(nodes2, "A", "P")
result2 = astar2.search()
self.assertEqual(result2, ['A', 'B', 'C', 'D', 'G', 'L', 'P'])
def run(self):
while not self.exit:
self.screen.fill((255, 255, 255))
self.draw_grid(self.screen, self.grid, self.grid_size, self.margin)
algorithm = Astar(self.grid, self.start, self.end)
path = algorithm.astar()
for event in pygame.event.get():
if event.type == pygame.QUIT:
self.exit = True
if path == 'failure':
print('failure')
else:
for path_node in path:
self.show_node(self.screen, (52, 158, 235), path_node.x,
path_node.y, self.margin)
pygame.display.flip()
self.clock.tick(self.ticks)
pygame.quit()
numlist = line.split(' ')
if len(numlist) < 15:
return
problems.append(Puzzle([int(x) for x in numlist[1:]]))
print('%5s%10s%10s%10s%10s' % ('#prob', '#exp', '#gen', '|sol|', 'tiempo'))
problems = []
load_problems(problems)
total_time = 0
total_cost = 0
total_expansions = 0
total_problems = len(problems) # cambiar si quieres menos problemas
for prob in range(0, total_problems):
init = problems[prob] # problema aleatorio
s = Astar(init, heuristic, 1)
result = s.search()
print('%5d%10d%10d%10d%10.2f' % (prob + 1, s.expansions, len(
s.generated), result.g, s.end_time - s.start_time))
total_time += s.end_time - s.start_time
total_expansions += s.expansions
total_cost += result.g
if show_solutions:
print(result.trace())
print('Total time: %.3f' % (total_time))
print('Expansiones totales: %d' % (total_expansions))
print('Total cost: %.3d' % (total_cost))
def setUp(self):
self.searcher1 = Astar((0, 0), (10, 10), 10)
self.searcher2 = Astar((-5, -5), (5, 5), 10)
self.searcher3 = Astar((0, 0), (10, 10), 100)
def main():
global corner1, corner2, numCells
global searcher, newPath
rospy.init_node('astar_alpha_main')
# set the parameters specified within the launch files
# corner 1 and corner 2 specify the area in the map
# that astar will consider
# Corner1
if rospy.has_param('corner1x'):
corner1x = rospy.get_param('corner1x')
else:
corner1x = -6.25
if rospy.has_param('corner1y'):
corner1y = rospy.get_param('corner1y')
else:
corner1y = 8.2
# Corner2
if rospy.has_param('corner2x'):
corner2x = rospy.get_param('corner2x')
else:
corner2x = 15.75
if rospy.has_param('corner2y'):
corner2y = rospy.get_param('corner2y')
else:
corner2y = 28.2
corner1 = (corner1x, corner1y)
corner2 = (corner2x, corner2y)
# numCells specifies the number of cells to divide the
# specified astar search space into
if rospy.has_param('numCells'):
numCells = rospy.get_param('numCells')
else:
numCells = 100
# topic that the node looks for the closed points on
if rospy.has_param('inflatedTopic'):
inflatedTopic = rospy.get_param('inflatedTopic')
else:
inflatedTopic = '/costmap_local_alpha/costmap_local/inflated_obstacles'
# topic that the node looks for goal messages on
if rospy.has_param('goalTopic'):
goalTopic = rospy.get_param('goalTopic')
else:
goalTopic = 'goal_point'
# initialize an instance of the Astar class
searcher = Astar(corner1, corner2, numCells)
naptime = rospy.Rate(RATE)
print "corner1: "
print corner1
print ""
print "corner2: "
print corner2
print ""
print "numCells: %i" % numCells
print ""
print "goal topics: %s" % goalTopic
print ""
print "inflatedTopic: %s" % inflatedTopic
rospy.Subscriber(goalTopic, GoalMsg, goalCallback)
rospy.Subscriber(inflatedTopic, GridCellsMsg, inflatedObstaclesCallback)
rospy.Subscriber('map_pos', PoseStampedMsg, poseCallback)
pathPointPub = rospy.Publisher('point_list', PointListMsg)
first_run = True
pointList = PointListMsg()
while not rospy.is_shutdown():
pointList.new = newPath
print "searcher.start"
print searcher.start
print ""
print "searcher.goal"
print searcher.goal
print ""
print "newPath"
print newPath
print ""
print "searcher.path"
print searcher.path
print ""
pointList.points = []
for point in searcher.path:
pathPoint = PointMsg()
pathPoint.x = point[0]
pathPoint.y = point[1]
pointList.points.append(pathPoint)
pathPointPub.publish(pointList)
if newPath:
newPath = False
naptime.sleep()
altitude, safe_distance = 5, 3
self.grid = create_grid(data, altitude, safe_distance)
def get_grid(self):
return self.grid
def show(self, x_values, y_values):
skeleton = medial_axis(invert(self.grid))
plt.imshow(self.grid, origin='lower')
plt.imshow(skeleton, cmap='Greys', origin='lower', alpha=0.7)
plt.xlabel('EAST')
plt.ylabel('NORTH')
plt.plot(x_values, y_values, 'g')
plt.show()
start_ne = (25, 100)
goal_ne = (750., 370.)
medial = Medial('colliders.csv')
grid = medial.get_grid()
astar = Astar(grid)
found, paths = astar.travel(start_ne, goal_ne)
path = astar.trace_back(paths) if found else exit("Couldn't find a path")
xpoints, ypoints = astar.axis_points(path)
medial.show(xpoints, ypoints)
Puzzle.initialize_pdb(3) # heuristica basda en distancia manhattan
heuristic = Puzzle.pdb_best # una heurística basada en pattern databases que debes implementar
# inicialización de la heurística basada en PDBs
print('%5s%10s%10s%10s%10s%10s' %
('#prob', '#exp', '#gen', '|sol|', 'tiempo', 'maxsubopt'))
problems = []
load_problems(problems)
total_time = 0
total_cost = 0
total_expansions = 0
#num_problems = len(problems) # cambiar si quieres ejecutar sobre todos los problemas
num_problems = 10 # solo ejecutamos los primeros 10 problemas
for prob in range(0, num_problems):
init = problems[prob]
s = Astar(
init, heuristic, 2
) # agregar un tercer parámetro una vez que lo hayas transformado en Weighted A*
result = s.search()
print('%5d%10d%10d%10d%10.2f%10.2f' %
(prob + 1, s.expansions, len(s.generated), result.g,
s.end_time - s.start_time, s.estimate_suboptimality()))
total_time += s.end_time - s.start_time
total_cost += result.g
total_expansions += s.expansions
if show_solutions:
print(result.trace())
print('Tiempo total: %.2f' % (total_time))
print('Expansiones totales: %d' % (total_expansions))
print('Costo total: %d' % (total_cost))
def run_astar(episode):
MAX_RUNS = 2000000
TURN_ANGLE = 30
FORWARD_STEP_SIZE = 0.25
AGENT_HEIGHT = 0.88
AGENT_RADIUS = 0.18
house_floor_threshold = {
'2azQ1b91cZZ': [1.5],
'8194nk5LbLH': [1.2],
'EU6Fwq7SyZv': [-1.3, 2.0],
'oLBMNvg9in8': [-0.8, 1.8, 4.6],
'pLe4wQe7qrG': [],
'QUCTc6BB5sX': [-1.5],
'TbHJrupSAjP': [-1.5, 1.7],
'X7HyMhZNoso': [1.8],
'x8F5xyUWy9e': [],
'Z6MFQCViBuw': [],
'zsNo4HB9uLZ': [],
}
# -- folders
data_dir = '../data/ObjectNav/objectnav_mp3d_v1/val/'
folder_pred = '../data/ObjectNav/semmap/'
floormap_dir = '../data/ObjectNav/freespace_map/'
info = json.load(open('../data/ObjectNav/semmap_objnav_info.json', 'r'))
# -- setup naming bindings
read_tsv = csv.reader(open("mpcat40.tsv"), delimiter="\t")
mpcat40 = {line[0]: line[1] for line in read_tsv}
jsonfile = json.load(open(os.path.join(data_dir, 'val.json'), 'r'))
category_to_mp3d_category_id = jsonfile['category_to_mp3d_category_id']
house = episode['scene_id'].split('/')[1]
# -- sample object category target
obj_semantic_id = category_to_mp3d_category_id[episode['object_category']]
obj_semantic_name = mpcat40[str(obj_semantic_id)]
if obj_semantic_name in object_whitelist:
sid = object_whitelist.index(obj_semantic_name) + 1
else:
return (episode['episode_id'], [], [])
# -- select floor -> env
if house_floor_threshold[house]:
level = bisect.bisect(house_floor_threshold[house],
episode['start_position'][1])
else:
level = 0
env = house + '_' + str(level)
map_world_shift = np.array(info[env]['map_world_shift'])
# -- load maps
floormap = imread(os.path.join(floormap_dir, env + '.png'))
floormap = floormap.astype(np.float)
floormap /= 255
floormap = floormap.astype(np.bool)
if not os.path.isfile(os.path.join(folder_pred, env + '.h5')):
return (episode['episode_id'], [], [])
h5file = h5py.File(os.path.join(folder_pred, env + '.h5'), 'r')
map_semantic = np.array(h5file['semmap'])
observed_map = np.array(h5file['observed_map'])
observed_map = observed_map.astype(np.float)
h5file.close()
map_semantic = np.multiply(map_semantic, observed_map)
map_semantic = map_semantic.astype(np.int)
#goals = None
goals = all_goals[house][episode['episode_id']]
goals = np.array(goals)
goals -= map_world_shift
goals = goals[:, [0, 2]]
# -- get init position
start_pos = episode['start_position']
start_pos -= map_world_shift
start_x = start_pos[0]
start_y = start_pos[2]
start_row = int(np.round(start_y / resolution))
start_col = int(np.round(start_x / resolution))
start_rot = np.array(episode['start_rotation'])
r = R.from_quat(start_rot)
_, start_heading, _ = r.as_rotvec()
start_heading = (start_heading * 180 / np.pi)
start_heading = -(start_heading + 90)
start = Node(x=start_x,
y=start_y,
heading=start_heading,
row=start_row,
col=start_col)
# -- get maps for Astar
goal_mask = map_semantic == sid
goal_mask = binary_opening(goal_mask.astype(int),
structure=np.ones((3, 3))).astype(np.bool)
floormap = binary_closing(floormap.astype(int),
structure=np.ones((10, 10))).astype(np.bool)
navmap = floormap & (map_semantic == 0)
# compute Heuristic
# -- Euclidean distance
non_object_mask = ~goal_mask
non_object_mask = non_object_mask.astype(np.uint8)
distance_map = cv2.distanceTransform(non_object_mask.copy(), cv2.DIST_L2,
3)
# -- Geodesic distance
#os.system('pip install scikit-fmm')
#import skfmm
#import numpy.ma as ma
#mask = ~navmap&~goal_mask
#mask = ~mask
#mask = binary_dilation(mask.astype(int), structure=np.ones((10,10))).astype(np.bool)
#mask = ~mask
#map = np.ones(navmap.shape)
#map = map - 2*goal_mask.astype(np.float)
#map = ma.masked_array(map, mask)
#distance_map = skfmm.distance(map)
pathfinder = Astar(navmap,
observed_map,
heuristic=distance_map,
init_heading=start_heading,
goals=goals)
path, runs = pathfinder.run(start, goal_mask, max_runs=MAX_RUNS)
if len(path) == 0:
actions = []
else:
path = path[::-1]
# -- convert path to actions
actions = []
prev_p = path[0]
for p in path[1:]:
# -- rotate
pre_h = prev_p[2]
new_h = p[2]
delta_h = (new_h - pre_h + 360) % 360
if delta_h == 0:
pass
elif delta_h <= 180:
#trun right
num_rotations = int(delta_h / TURN_ANGLE)
actions += [3] * abs(num_rotations)
elif delta_h > 180:
#trun left
delta_h = 360 - delta_h
num_rotations = int(delta_h / TURN_ANGLE)
actions += [2] * abs(num_rotations)
# move forward
actions.append(1)
prev_p = p
actions.append(0)
return (episode['episode_id'], actions, path)
def runAlgorithm(algo):
file = open(inputPath,"r")
afile = open("analysis.csv","a")
afile.write("\nAlgorithm, Puzzle, Cost, Length Of Solution, Length Of Search, Execution, No Solution, Optimality\n")
goalstate1 = "1 2 3 4 5 6 7 0"
goalstate2 = "1 3 5 7 2 4 6 0"
totalLengthOfSolution = 0
totalLengthOfSearch = 0
totalNoSolution = 0
totalCost = 0
totalExecutionTime = 0
counter = 0
averageCost = averageExecutionTime = averageLengthOfSearch = averageLengthOfSolution = averageNoSolution = 0
i = 0
for line in file:
lengthOfSolution = 0
lengthOfSearch = 0
duration = 0
cost = 0
analysis = None
if algo == ucs:
analysis = algoAnalysis(UniformCostSearch(puzzleNumber = i, initial = line.strip(), goal_state1 = goalstate1,goal_state2 = goalstate2))
elif algo == gbfsH0:
analysis = algoAnalysis(GBFS(num = i, input = line.strip(), heuristic="h0", openList = [], closedList = [], goalState1=goalstate1, goalState2=goalstate2))
elif algo == gbfsH1:
analysis = algoAnalysis(GBFS(num = i, input = line.strip(), heuristic="h1", openList = [], closedList = [], goalState1=goalstate1, goalState2=goalstate2))
elif algo == gbfsH2:
analysis = algoAnalysis(GBFS(num = i, input = line.strip(), heuristic="h2", openList = [], closedList = [], goalState1=goalstate1, goalState2=goalstate2))
elif algo == gbfsH3:
analysis = algoAnalysis(GBFS(num = i, input = line.strip(), heuristic="h3", openList = [], closedList = [], goalState1=goalstate1, goalState2=goalstate2))
elif algo == astarH0:
analysis = algoAnalysis(Astar(puzzleNumber = i, input = line.strip(), heuristic="h0", goalState1=goalstate1, goalState2=goalstate2, monotonic=False))
elif algo == astarH1:
analysis = algoAnalysis(Astar(puzzleNumber = i, input = line.strip(), heuristic="h1", goalState1=goalstate1, goalState2=goalstate2, monotonic = False))
elif algo == astarH2:
analysis = algoAnalysis(Astar(puzzleNumber = i, input = line.strip(), heuristic="h2", goalState1=goalstate1, goalState2=goalstate2, monotonic=True))
elif algo == astarH3:
analysis = algoAnalysis(Astar(puzzleNumber = i, input = line.strip(), heuristic="h3", goalState1=goalstate1, goalState2=goalstate2, monotonic=False))
duration = analysis[5]
lengthOfSolution = analysis[1]
lengthOfSearch = analysis[2]
if analysis[3] == 0:
cost = analysis[4]
counter = counter + 1
totalCost = totalCost+cost
totalExecutionTime = totalExecutionTime + duration
totalLengthOfSearch = totalLengthOfSearch + lengthOfSearch
if lengthOfSolution != 0:
totalLengthOfSolution = totalLengthOfSolution + lengthOfSolution
else:
totalNoSolution = totalNoSolution + 1
afile.write(algo +", "+ str(i)+", "+str(cost)+", "+str(lengthOfSolution)+", "+str(lengthOfSearch)+", "+str(duration)+","+str(analysis[3])+",,"+line.strip()+"\n")
i = i + 1
afile.write("Total-"+algo+", , "+str(totalCost)+","+str(totalLengthOfSolution)+", "+str(totalLengthOfSearch)+", "+str(totalExecutionTime)+","+str(totalNoSolution)+"\n")
if(i != 0 and counter != 0):
averageCost = totalCost/counter
averageExecutionTime = totalExecutionTime/counter
averageLengthOfSearch = totalLengthOfSearch/i
averageLengthOfSolution = totalLengthOfSolution/counter
averageNoSolution = totalNoSolution/i
afile.write("Average-"+algo+", , "+str(averageCost)+","+str(averageLengthOfSolution)+", "+str(averageLengthOfSearch)+", "+str(averageExecutionTime)+","+str(averageNoSolution)+"\n")
file.close()
afile.close()
node7 = Node('Node 7', x_position=90, y_position=90)
node8 = Node('Node 8', x_position=120, y_position=120)
# create connections
node1.add_connection(node2, 7)
node1.add_connection(node3, 2)
node2.add_connection(node4, 5)
node3.add_connection(node5, 3)
node4.add_connection(node6, 1)
node5.add_connection(node7, 3)
node5.add_connection(node8, 4)
node7.add_connection(node6, 3)
node8.add_connection(node7, 1)
# do search
ucs = Ucs()
print_result_info(ucs.run(start=node1, goal=node6))
Node.reset_nodes()
astar = Astar(heuristic_factor=0.05, heuristic=Astar.MANHATTAN_DISTANCE)
print_result_info(astar.run(start=node1, goal=node6))
return canvas
def draw(self, canvas):
self.im.set_data(canvas)
self.fig.canvas.draw_idle()
plt.pause(0.000001)
def show(self, canvas):
self.im.set_data(canvas)
# self.fig.canvas.draw_idle()
# plt.pause(.001)
grid = Grid(25, 25)
grid.wall()
astar = Astar(grid, grid.start, grid.finish, (0, 0, 0, 1))
astar.search()
user = input('exit? - enter any key')
# agents = []
# num_agent = 10
# for i in range(num_agent):
# agents.append(Agent(grid, grid.start, (0, 0, 1, 1)))
# agents.append(Agent(grid, grid.finish, (1, 0, 0, 1)))
#
# fig, ax = plt.subplots(1, 1)
# im = ax.imshow(grid.update())
# plt.pause(.1)
# while agents[0].searching:
# grid.score()
# -*- coding: utf-8 -*-
from environment import Environment
from dijkstra import Dijkstra
from astar import Astar
if __name__ == "__main__":
resolution = 0.1
botRadius = 0
env = Environment(resolution, botRadius)
env.render()
alg = Dijkstra(env)
alg.executeAlg()
env.reset()
alg1 = Astar(env)
alg1.executeAlg()
def mouse_event(self, event):
"""
"""
if event.type == MOUSEBUTTONDOWN:
mouse_pos = (event.pos[0] / SQUARE_SIZE,
event.pos[1] / SQUARE_SIZE)
if self.mouse_over((WIDTH - 18, 0, 18, 18), event.pos):
self.render.menu = not self.render.menu
elif not self.render.menu and event.button == 1:
if self.round_state == 'CHOICE' and not self.spec and self.render.l_entities[
self.render.me].alive:
if self.mouse_over((WIDTH / 2 - 29, HEIGHT - 26, 16, 16),
event.pos):
self.attack = 'attack'
elif self.mouse_over((WIDTH / 2 - 8, HEIGHT - 26, 16, 16),
event.pos):
self.attack = 'pyrotechnic'
elif self.mouse_over((WIDTH / 2 + 13, HEIGHT - 26, 16, 16),
event.pos):
self.attack = 'windblow'
elif self.mouse_over((20, HEIGHT - 40, 90, 20), event.pos):
self.buffer_pa.clear()
self.render.path = None
self.render.target = None
elif self.mouse_over((20, HEIGHT - 70, 90, 20), event.pos):
while len(self.buffer_pa):
self.render.s_queue.put(self.buffer_pa.popleft())
self.render.s_queue.put("CONFIRM_CHOICE")
self.confirm = True
elif self.entity_on(mouse_pos) is not None:
if self.attack == 'attack' and self.reachable(
mouse_pos,
MELEE_RANGE) and self.render.target is None:
self.buffer_pa.append('ATTACK:attack:%d:%d' %
mouse_pos)
self.render.target = self.render.l_entities.get_by_pos(
mouse_pos)
elif self.attack == 'pyrotechnic' and self.reachable(
mouse_pos,
PYRO_RANGE) and self.render.target is None:
self.buffer_pa.append('ATTACK:pyrotechnic:%d:%d' %
mouse_pos)
self.render.target = self.render.l_entities.get_by_pos(
mouse_pos)
elif self.attack == 'windblow' and self.reachable(
mouse_pos,
WIND_RANGE) and self.render.target is None:
self.buffer_pa.append('ATTACK:windblow:%d:%d' %
mouse_pos)
self.render.target = self.render.l_entities.get_by_pos(
mouse_pos)
elif self.render.path is None and self.reachable(
mouse_pos, MOVE_DIST):
self.buffer_pa.append('MOVE:%d:%d' % mouse_pos)
start_pos = self.render.l_entities[self.render.me].pos
self.render.path = (Astar(self.get_collide_map(),
start_pos, mouse_pos, [0],
[BLOCK]))
elif self.round_state == 'PLAYERS_CONNECTION' and self.mouse_over(
((WIDTH - 200) / 2, (HEIGHT - 50) / 2, 200, 50),
event.pos) and not self.render.rdy:
self.render.rdy = True
self.render.s_queue.put("READY_TO_PLAY")
else:
menu_x = (WIDTH - MENU_WIDTH) / 2
menu_y = (HEIGHT - MENU_HEIGHT) / 2
if self.mouse_over((menu_x + 30, menu_y + 50, 10, 10),
event.pos):
self.render.grid_render = not self.render.grid_render
elif self.mouse_over((menu_x + 30, menu_y + 90, 10, 10),
event.pos):
self.render.fps_render = not self.render.fps_render
elif self.mouse_over(
(menu_x + MENU_WIDTH - 16, menu_y + 3, 13, 13), event.pos):
self.render.menu = not self.render.menu
elif self.mouse_over((menu_x + 410, menu_y + 270, 82, 20),
event.pos):
pygame.event.post(pygame.event.Event(QUIT, {}))
elif event.type == MOUSEMOTION and self.render.me is not None:
mouse_pos = (event.pos[0] / SQUARE_SIZE,
event.pos[1] / SQUARE_SIZE)
if self.entity_on(
mouse_pos) is not None and self.round_state == 'CHOICE':
if self.attack == 'attack' and self.reachable(
mouse_pos, MELEE_RANGE):
self.render.use_cursor(SWORD)
elif self.attack == 'pyrotechnic' and self.reachable(
mouse_pos, PYRO_RANGE):
self.render.use_cursor(PYRO)
elif self.attack == 'windblow' and self.reachable(
mouse_pos, WIND_RANGE):
self.render.use_cursor(WIND)
elif self.render.cursor != ARROW[0]:
self.render.use_cursor(ARROW)
while (time.time() - start <= 0.75):
self.data.angular.z = 0
self.data.linear.x = lin
self.pub.publish(self.data)
self.loop_rate.sleep()
pass
if __name__ == '__main__':
rospy.init_node("botmove", anonymous=False)
init_x = rospy.get_param('~init_x')
init_y = rospy.get_param('~init_y')
init_theta = rospy.get_param('~init_t')
clrn = [float(rospy.get_param('~clr'))]
my_node = Node()
initCord = [init_x, init_y, init_theta]
actionSet, step = Astar(initCord, clrn)
print("\nactions--\n\n", actionSet, "\nstep\n", step)
time.sleep(2)
if (len(actionSet) != 0):
for angle in actionSet[1:]:
x = step * 1.0 / 50
if angle > 180:
angle = angle - 360
z = angle * 3.14 / 180
my_node.pubData(0.7 * x, z)
# my_node.pubData(0.812*x,0.965*z)
def main():
pygame.init()
# Taking input from user
n = int(input("Enter grid size:"))
Sx, Sy = [int(x) for x in input("Enter knight coords:").split()]
Tx, Ty = [int(x) for x in input("Enter queen coords:").split()]
# Setting up the screen and images
res = (n*pixels,n*pixels)
gameDisplay = pygame.display.set_mode(res)
queen = pygame.image.load("queen.png")
knight = pygame.image.load("knight.png")
# Initializing the board and the algo
createChessboard(gameDisplay,n)
placeEntity(queen,Ty,Tx,gameDisplay)
d = Astar(n,Sx,Sy,Tx,Ty)
# game loop
running = True
while running:
if not d.found:
# returns current node and previous node
t, prev = d.search()
placeEntity(knight,t[1],t[0],gameDisplay)
markVisited(prev[1],prev[0],gameDisplay)
pygame.display.update()
time.sleep(.2)
if t == (Tx,Ty):
createChessboard(gameDisplay,n)
placeEntity(queen,Ty,Tx,gameDisplay)
placeEntity(knight,Sy,Sx,gameDisplay)
path = d.createPath()
for i,j in path[1:]:
markVisited(j,i,gameDisplay)
pygame.display.update()
continue
for event in pygame.event.get():
if event.type == pygame.QUIT:
running = False
break
print("Target position ",(Tx,Ty)," reached: ",d.visited[Tx][Ty])
pygame.quit()
def execute_algo(self):
for event in pygame.event.get():
if event.type == pygame.QUIT:
self.running = False
# BFS
if self.algorithm_state == 'bfs':
self.bfs = BreadthFirst(self, self.start_node_x, self.start_node_y,
self.end_node_x, self.end_node_y,
self.wall_pos)
if self.start_node_x or self.end_node_x is not None:
self.bfs.bfs_execute()
if self.bfs.route_found:
self.draw_path = VisualizePath(self.screen, self.start_node_x,
self.start_node_y,
self.bfs.route, [])
self.draw_path.get_path_coords()
self.draw_path.draw_path()
else:
self.draw_text('NO ROUTE FOUND!',
self.screen, [768, 384],
50,
RED,
FONT,
center=True)
# DFS
elif self.algorithm_state == 'dfs':
self.dfs = DepthFirst(self, self.start_node_x, self.start_node_y,
self.end_node_x, self.end_node_y,
self.wall_pos)
if self.start_node_x or self.end_node_x is not None:
self.dfs.dfs_execute()
if self.dfs.route_found:
self.draw_path = VisualizePath(self.screen, self.start_node_x,
self.start_node_y,
self.dfs.route, [])
self.draw_path.get_path_coords()
self.draw_path.draw_path()
else:
self.draw_text('NO ROUTE FOUND!',
self.screen, [768, 384],
50,
RED,
FONT,
center=True)
# ASTAR
elif self.algorithm_state == 'astar':
self.astar = Astar(self, self.start_node_x, self.start_node_y,
self.end_node_x, self.end_node_y, self.wall_pos)
if self.start_node_x or self.end_node_x is not None:
self.astar.astar_execute()
if self.astar.route_found:
self.draw_path = VisualizePath(self.screen, self.start_node_x,
self.start_node_y, None,
self.astar.route)
self.draw_path.draw_path()
else:
self.draw_text('NO ROUTE FOUND!',
self.screen, [768, 384],
50,
RED,
FONT,
center=True)
# DIJKSTRA
elif self.algorithm_state == 'dijkstra':
self.dijkstra = Dijkstra(self, self.start_node_x,
self.start_node_y, self.end_node_x,
self.end_node_y, self.wall_pos)
if self.start_node_x or self.end_node_x is not None:
self.dijkstra.dijkstra_execute()
if self.dijkstra.route_found:
self.draw_path = VisualizePath(self.screen, self.start_node_x,
self.start_node_y, None,
self.dijkstra.route)
self.draw_path.draw_path()
else:
self.draw_text('NO ROUTE FOUND!',
self.screen, [768, 384],
50,
RED,
FONT,
center=True)
pygame.display.update()
self.state = 'aftermath'
def makepath(self, wrld):
pathMaker = Astar(wrld)
return pathMaker.findpathtoend(self.x, self.y)
plt.scatter(n1[1] - emin, n1[0] - nmin, c='blue')
# draw connected nodes
for n1 in g.nodes:
plt.scatter(n1[1] - emin, n1[0] - nmin, c='red')
plt.xlabel('NORTH')
plt.ylabel('EAST')
start = list(g.nodes)[0]
k = np.random.randint(len(g.nodes))
print(k, len(g.nodes))
goal = list(g.nodes)[k]
print('start = ', start, ' goal = ', goal)
astar = Astar(g)
found, paths = astar.travel(start, goal)
if found is False: exit("Unable to find path")
path = astar.trace_back(paths)
print(path)
path_pairs = zip(path[:-1], path[1:])
for (n1, n2) in path_pairs:
plt.plot([n1[1] - emin, n2[1] - emin], [n1[0] - nmin, n2[0] - nmin],
'green')
plt.show()
ax.set_ylim3d([-300.0, 300.0])
ax.set_zlim3d([-100.0, 300.0])
map_struct = loadmap("maps/final.txt")
obstacles = map_struct.obstacles
start = np.array([0, 0, 0, 0, 0, 0])
goal = np.array([-1, 0, 1, 0, 0, 0])
max_nodes = 5000
max_iter = 3000
# path, cost = rrt(deepcopy(map_struct), deepcopy(start), deepcopy(goal), max_nodes, max_iter,
# stepsize=0.1, neighbour_radius=0.15, bias_ratio=5, bias_radius=0.075,
# optimize=True)
path = Astar(deepcopy(map_struct), deepcopy(start), deepcopy(goal))
fk = calculateFK()
start_pos = fk.forward(path[0])[0]
line = ax.plot(start_pos[:, 0], start_pos[:, 1], start_pos[:, 2])[0]
visualize.plot_obstacles(ax, obstacles)
arm_ani = animation.FuncAnimation(fig,
visualize.animate,
fargs=(path, line),
interval=20,
blit=False)
plt.show()
use_astar = True
elif op in ("-d", "--dstar"):
use_dstar = True
if use_astar is None and use_dstar is None:
print("Error: You need to select one algorithm to plan path: Astar or Dstar!")
sys.exit()
else:
print('Space: ', space_boundary)
print('Agents number: ', agents_num)
print('Safe Radius: ', safe_R)
print('Seed: ', seed)
env = Env(space_boundary = space_boundary,
agents_num=agents_num, seed = seed, safe_R = safe_R)
if use_astar:
print('Algorithm: Astar')
astar = Astar(env.agents_pos[:, 0], env.agents_targ[:, 0], env.agents_pos,
env.space_boundary, env.walk_dirs)
pathData = astar.search()
pathsData = {"Plan Path": pathData}
draw_path(env.space_boundary ,env.agents_pos, pathsData,
env.agents_targ, title = "Path Plan with A*")
elif use_dstar:
print('Algorithm: Dstar')
space_map = Map(env.space_boundary, env.walk_dirs)
dstar = Dstar(space_map, env.space_boundary)
paths = dstar.search( env.agents_pos[:, 0], env.agents_targ[:, 0], env.agents_pos)
pathsData = {"Path Without Obstacle": paths[0], "Path With Obstacle": paths[1]}
draw_path(env.space_boundary ,env.agents_pos, pathsData,
env.agents_targ, title = "Path Plan with D*")
print("Finish!")
sys.exit()
def render_move(self):
"""
Simulate and execute all "move" actions and prepare the request to send to Etain.
"""
# list of (client_id, src_pos, dest_pos, pa_start)
# where:
# client_id is the client identifier.
# src_pos is the tuple (x, y) which represent initial position before the move.
# dest_pos is the tuple (x, y) which represent the position where the entity want to go.
# pa_start is the action number where the move start.
move_actions = self.get_move_actions()
# build move flow by actions number
self.move_flow = []
finished_move_actions = set()
for pa in xrange(NB_ACTIONS):
moves_by_pa = {}
for move_id, move in enumerate(move_actions):
str_client_id, str_src_pos, str_dest_pos, str_pa_start = move
client_id = int(str_client_id)
src_pos = self.world.get_position_by_object_id(client_id)
# src_pos = (int(str_src_pos[0]), int(str_src_pos[1]))
dest_pos = (int(str_dest_pos[0]), int(str_dest_pos[1]))
pa_start = int(str_pa_start)
# if the move has started and isn't finished, then calculate it
if move_id not in finished_move_actions and pa_start <= pa:
# build A* arguments
# we need a world representation which show where
# the blocking squares are
#
# copy world representation
world_representation = []
for line in self.world.map:
line_clone = []
for item in line:
line_clone.append(item)
world_representation.append(line_clone)
# and add entity as blocking square
for entity_x, entity_y in self.world.entities_pos.values():
if entity_y <= len(
world_representation) and entity_x <= len(
world_representation[0]):
world_representation[entity_y][entity_x] = 1
else:
# ignore entity which are outside the screen
pass
free = [0]
block = [1]
# we have to re calculate the path to follow for
# each pa to be able to adapt the path to other
# entities' moves.
print 'A START MAGIC'
a_star_pos_list = Astar(world_representation, src_pos,
dest_pos, free, block)
pos = a_star_pos_list[0] # first move of one square
pos_x, pos_y = pos
# collision detection
if self.world.square_available(pos_x, pos_y):
print 'client n° %d move to (%d, %d)' % (client_id,
pos_x, pos_y)
self.world.move(client_id, pos_x, pos_y)
# generate move action of 1 square
moves_by_pa[client_id] = (pos_x, pos_y)
if pos == dest_pos: # is the move finished ?
print('move is finished') #DEBUG
finished_move_actions.add(move_id)
else:
# collision detected
# generate "non-move" action
moves_by_pa[client_id] = (None, None)
else:
print("move is finished or isn't started yet")
# end for move in move_actions
print('fin iteration PA : moves_by_pa = ', moves_by_pa)
self.move_flow.append(moves_by_pa)
# end for pa in range(NB_ACTIONS)
print("move_flow", self.move_flow)
import sys
from map import Map
from astar import Astar
if __name__ == '__main__':
size = len(sys.argv)
if size == 2:
try:
_file = open(sys.argv[1], "r")
content = _file.read()
_file.close()
map = Map(content)
except:
print("The file is unknown")
if map.getStepNumber() >= 2:
Astar(map)
else:
print("The map must have 2 steps at least")
else:
print("The path of the file is required")
for p in graph.nodes:
sub = np.subtract(p, current_point)
d = np.linalg.norm(sub)
if d < dist:
closest_point = p
dist = d
return closest_point
start_ne = (25, 100)
goal_ne = (750., 370.)
voronoi = VoronoiGraph('colliders.csv')
graph = voronoi.create_graph()
start_ne = closest_point(graph, start_ne)
goal_ne = closest_point(graph, goal_ne)
print('################')
print(voronoi.edges)
print('################')
astar = Astar(graph)
found, paths = astar.travel(start_ne, goal_ne)
path = astar.trace_back(paths) if found else exit("Couldn't find a path")
xpoints, ypoints = astar.axis_points(path)
voronoi.show(xpoints, ypoints)
