def main():
# ヒューリスティクス関数のリスト
heuristics_func_list = [
calc_euclidean_distance, calc_manhattan_distance, all_0
]
## 標準入出力を利用し、対話的に設定を行う
print("# 読み込むファイル名を入力してください(何も入力しない場合はmap.csvの読み込みを試みます)")
f_name = input()
if f_name == '': a_star = AStar()
else: a_star = AStar(f_name=f_name)
a_star.print_map()
print("# スタートの位置を空白区切りで入力してください(X, Yの順)")
start = tuple(map(int, input().split()))
print("# ゴールの位置を空白区切りで入力してください(X, Yの順)")
goal = tuple(map(int, input().split()))
print("# 使用するヒューリスティクス関数を以下の番号から指定してください")
for i in range(len(heuristics_func_list)):
print("# %d: %s" % (i, heuristics_func_list[i].__name__))
func_index = int(input())
route = a_star.search(start, goal, heuristics_func_list[func_index])
print("route: ", end='')
for x, y in route:
print("[%s, %s], " % (x, y), end='')
print("")
def __init__(self):
self.a_star = AStar()
self.imu = LSM6()
self.g_y_zero = 0
self.angle = 0 # degrees
self.angle_rate = 0 # degrees/s
self.distance_left = 0
self.speed_left = 0
self.drive_left = 0
self.last_counts_left = 0
self.distance_right = 0
self.speed_right = 0
self.drive_right = 0
self.last_counts_right = 0
self.motor_speed = 0
self.balancing = False
self.calibrated = False
self.running = False
self.next_update = 0
self.update_thread = None
self.cur_cmd = balance_cmd()
self.cur_status = 0
self.adj_speed_left = 0
self.adj_speed_right = 0
self.lock = threading.Lock()
def solve():
global cont_solve, animation, drawn_spaces, checked
file = "\n".join([
"".join([
"e" if s.end else ("s" if s.start else ("#" if s.wall else " "))
for s in row
]) for row in grid
])
with open("maze.txt", "w") as f:
f.write(file)
a = AStar(grid_w, grid_h, True, False)
path, checked = a.solve()
for row in grid:
for cell in row:
cell.path = False
cell.checked = False
if path is not None:
cont_solve = True
drawn_spaces = []
for p in path:
grid[p.y][p.x].path = True
for c in checked:
try:
grid[c.y][c.x].checked = True
except Exception as e:
print(e, c.x, c.y)
else:
print("NOT SOLVEABLE")
def __init__(self,
graph,
agents,
model_name,
volume_conflict_table,
min_cost_table=0):
self.graph = graph
self.volume_conflict_table = volume_conflict_table
if min_cost_table != 0:
self.min_cost_path_table = min_cost_table
else:
try:
with open(model_name + '.mct', 'r') as f:
data = f.read()
if data != '':
self.min_cost_path_table = ast.literal_eval(data)
except FileNotFoundError:
dj = Dijkstra(graph)
dj.traverse()
self.min_cost_path_table = dj.paths
with open(model_name + '.mct', 'w') as f:
f.write(str(self.min_cost_path_table))
self.agents = agents
self.agent_dict = {}
self.make_agent_dict()
self.constraints = Constraints()
self.constraint_dict = {}
self.a_star = AStar(self)
def __init__(self):
#Create a_star instance to talk to arduino based a_star Romi board
self.a_star = AStar()
#self.a_star = MockBot()
self.encoder_resolution = 12 #according to pololu encoder count
self.gear_reduction = 120 #according to romi website
self.wheel_diameter = 0.07 #70mm diameter wheels according to website
self.wheel_track = 0.141 #149mm outside to outside according to drawing, 8mm thick tires
self.motorMax = 400
#internal data
self.leftEncoder = None #last encoder state variables for odometry
self.rightEncoder = None
self.x = 0
self.y = 0
self.th = 0
self.leftMotor = 0
self.rightMotor = 0
#time variables for odometry pulled from http://github.com/hbrobotics/ros_arduino_bridge
self.rate = float(50) #hard coding this for now, will be param'd later
self.tickes_per_meter = self.encoder_resolution * self.gear_reduction / (self.wheel_diameter * pi)
now = rospy.Time.now()
self.then = now
self.t_delta = rospy.Duration(1.0 / self.rate)
self.t_next = now + self.t_delta
rospy.Subscriber("lmotor_cmd", Float32, self.lmotorCallback)
rospy.Subscriber("rmotor_cmd", Float32, self.rmotorCallback)
self.lwheelPub = rospy.Publisher('lwheel', Int16, queue_size=5)
self.rwheelPub = rospy.Publisher('rwheel', Int16, queue_size=5)
def solve_board(self):
while not self.is_solution_optimal():
algorithm = AStar(self.board, self.data)
self.solution = algorithm.solve_board()
self.update_weights()
self.write_output()
return self.solution
def __init__(self): self.directionFacing = 'north' self.maxSpeed = 100 self.aStar = AStar() self.tcs = Adafruit_TCS34725.TCS34725() self.initialLeftCount = self.aStar.read_encoders()[0] self.initialRightCount = self.aStar.read_encoders()[1]
def h_cost(self, s):
plan = AStar((s[0], s[1]), (self._e[0], self._e[1]), self._map_info)
if plan.run(display=False):
path = plan.reconstruct_path()
d = 0
for i in range(len(path) - 1):
d += self.distance(path[i], path[i + 1])
return d
def __init__(self, width, height):
pygame.init()
self.grid = Graph(27, 19)
self.screen = pygame.display.set_mode((width, height))
self.start_node = Node(None)
self.goal_node = Node(None)
self.astar = AStar(self.grid, self.start_node, self.goal_node)
self.visual_graph = GraphVisual(self.astar, 40, self.screen)
def __init__(self):
self.bridge = cv_bridge.CvBridge()
# cv2.namedWindow("window", 1)
self.image_sub = rospy.Subscriber('cv_camera/image_raw', Image,
self.image_callback)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
self.twist = Twist()
self.a_star = AStar()
def get_solve_graph(algo_enum: AlgoEnum):
solve_graph: BaseGlobalSearch = None
if algo_enum == AlgoEnum.IDS:
solve_graph = IDS(order_int_list, N)
elif algo_enum == AlgoEnum.BFS:
solve_graph = BFS(order_int_list, N)
elif algo_enum == AlgoEnum.A_STAR:
solve_graph = AStar(order_int_list, N)
return solve_graph
def __init__(self):
self.isLost = False
self.nextSearchDirection = 0 # (left: 0, right: 1)
self.veerSpeed = 30
self.maxSpeed = 70
self.aStar = AStar()
self.tcs = Adafruit_TCS34725.TCS34725()
self.initialLeftCount = self.aStar.read_encoders()[0]
self.initialRightCount = self.aStar.read_encoders()[1]
def performance_computation():
n_times = 100
adaptive = AdaptiveAStar(maze=def_mazes[1])
a_star = AStar(maze=def_mazes[1])
reverse_a_star = ReverseAStar(maze=def_mazes[1])
all_a_star_moves = []
all_adaptive_moves = []
all_reverse_moves = []
adaptive_a_star_times = []
a_star_times = []
reverse_a_star_times = []
for _ in range(n_times):
new_start = a_star.maze.random_cell()
a_star.maze.start = new_start
a_star.reset_bot()
a_star_time_start = time.time()
a_star_pops, a_star_moves = a_star.run_a_star_with_bot()
a_star.pop_counter = 0
all_a_star_moves.append(a_star_moves)
a_star_times.append(round(time.time() - a_star_time_start, 4))
reverse_a_star_time_start = time.time()
reverse_a_star.reset_bot()
reverse_a_star.maze.start = new_start
reverse_pops, reverse_moves = reverse_a_star.run_reverse_a_star()
all_reverse_moves.append(reverse_moves)
reverse_a_star_times.append(round(time.time() - reverse_a_star_time_start, 4))
adaptive_time_start = time.time()
adaptive.maze.start = adaptive.maze.maze[a_star.maze.start.row][a_star.maze.start.col]
adaptive.reset_bot()
adaptive_pops, adaptive_moves = adaptive.execute_algorithm()
adaptive.pop_counter = 0
all_adaptive_moves.append(adaptive_moves)
adaptive_a_star_times.append(round(time.time() - adaptive_time_start, 4))
moves_obj = {
"adaptive_moves": all_adaptive_moves,
"a_star_moves": all_a_star_moves,
"reverse_moves": all_reverse_moves
}
times_obj = {
"adaptive_times": adaptive_a_star_times,
"a_star_times": a_star_times,
"reverse_times": reverse_a_star_times
}
with open("moves_new.json", "w") as moves_file:
json.dump(moves_obj, moves_file)
with open("times_new.json", "w") as times_file:
json.dump(times_obj, times_file)
def solve_board(self):
a_star = AStar(self.board, self.data)
root_state = State(self.board, None, None, 0, 0, 0)
f_limit = root_state.calculate_f(self.data)
while True:
solution_str = a_star.solve_board(f_limit)
if solution_str is not None:
return solution_str
f_limit = f_limit + 5
return None
def _get_path(start_x, start_y, end_x, end_y, cell_table):
# Columns and rows set to 0 because they are reassigned when the maze file is read
a = list(
reversed(AStar(0, 0).solve((start_y, start_x), (end_y, end_x))[0]))
# Reverse keys and values
cell_table = {v: k for k, v in cell_table.items()}
# Get the acc cell positions of path
return [cell_table[(a[i].x, a[i].y)] for i in range(0, len(a), 2)]
def solve_a_star(self):
world = deepcopy(self.world)
x = int(self.robot_x / self.cell_width)
y = int(self.robot_y / self.cell_height)
world[x][y] = -1
a_star = AStar(world,
start_point=(x, y),
end_point=(self.goal_x, self.goal_y))
a_star.a_star()
return a_star.get_path()
def __init__(self, a_star=None):
self.a_star = a_star or AStar()
self.encoders = Encoders(self.a_star)
self.odometer = Odometer(self.encoders)
self.motors = Motors(self.a_star, self.odometer)
self.camera = Camera()
self.log = logging.getLogger('romi')
self.motors.stop()
def run(self):
start_node = NavNode(position=self.board.start, g=0)
a_star = AStar(
draw=self.gfx.draw if not self.disable_gfx else lambda _: 0,
disable_gfx=self.disable_gfx,
draw_every=self.draw_every,
print_path=self.print_path
)
a_star.run(start_node=start_node)
def game_loop(start_point, fields_with_bombs):
# glowna petla
obj_fields = []
a = AStar()
current_field = start_point
field_to_move = fields_with_bombs[0]
fields_with_bombs.remove(field_to_move)
path = a.find_path(current_field, field_to_move, map_obj)
for y in game_map:
for x in y:
if not x.has_bomb:
r = random.randrange(200)
if r % 7 == 0:
print('Object position {}'.format(x.get_position()))
obj_fields.insert(len(obj_fields), x)
# gameDisplay.blit(rock_img, (x.map_x, x.map_y))
print('Ilosc obiektow: {}'.format(len(obj_fields)))
while True:
for event in pygame.event.get():
if event.type == pygame.QUIT:
quit()
for y in game_map:
for x in y:
gameDisplay.fill(x.color, (x.map_x, x.map_y, settings.FIELD_SIZE, settings.FIELD_SIZE))
if x.has_bomb:
gameDisplay.blit(bomba_img, (x.map_x, x.map_y))
if len(path) > 0:
current_field = path[0]
move_robot(current_field)
path.remove(current_field)
elif len(fields_with_bombs) > 0:
field_to_move = fields_with_bombs[0]
fields_with_bombs.remove(field_to_move)
path = a.find_path(current_field, field_to_move, map_obj)
move_robot(current_field)
if current_field == field_to_move:
make_action_with_bomb(current_field, settings.LABEL_FILE_1, settings.MODEL_FILE_1)
current_field.has_bomb = False
sleep(1)
if len(fields_with_bombs) == 0:
quit()
pygame.display.update()
clock.tick(3)
def initialize():
new_graph = gs.Graph()
setup.build_graph(new_graph)
a_star = AStar()
a_star.search(new_graph, '15', '35')
standard_links = a_star.standard_links
transition_links = a_star.transition_links
roundabout_links = a_star.roundabout_links
chart.render_chart(roundabout_links, transition_links, standard_links)
def keys_mtr(kmsg):
a_star = AStar()
if len(kmsg.data) == 0 or not key_mapping.has_key(kmsg.data[0]):
return
mtrs = key_mapping[kmsg.data[0]]
mtrs = np.array(mtrs, dtype='int16')
mleft = mtrs[0]
mright = mtrs[1]
# print(mleft, mleft.dtype)
# print(mright, mright.dtype)
a_star.motors(mleft, mright)
time.sleep(0.1)
def get_algorithm(algorithm_num, board, data):
if algorithm_num == 0:
return AStar(board, data)
elif algorithm_num == 1:
return IDAStar(board, data)
elif algorithm_num == 2:
return BiDirAStar(board, data)
elif algorithm_num == 3:
return ReinforcementLearning(board, data)
else:
print("Algorithm cannot be: " + str(algorithm_num))
exit(1)
def callback(directions):
a_star = AStar()
if directions.forward:
a_star.motors(200, 200)
elif directions.back:
a_star.motors(-200, -200)
elif directions.left:
a_star.motors(200, -200)
elif directions.right:
a_star.motors(-200, 200)
else:
a_star.motors(0, 0)
def __init__(self, dimension, agents, obstacles):
self.dimension = dimension
self.obstacles = obstacles
self.agents = agents
self.agent_dict = {}
self.make_agent_dict()
self.constraints = Constraints()
self.constraint_dict = {}
self.a_star = AStar(self)
class TestSensor(unittest.TestCase):
a_star = AStar()
tcs = Adafruit_TCS34725.TCS34725()
tcs.set_interrupt(False)
turnWheelSpeed=70
def testSensorColor(self):
self.assertTrue(getSensorColor(self.tcs)=='red' or getSensorColor(self.tcs)=='green' or getSensorColor(self.tcs)=='blue')
def testRecalibrateLeft(self):
calibrateLeft(self.a_star, self.tcs, 1000, self.turnWheelSpeed)
self.assertTrue(checkIfGreen(self.tcs))
def testRecalibrateRight(self):
calibrateRight(self.a_star, self.tcs, 1000, self.turnWheelSpeed)
self.assertTrue(checkIfGreen(self.tcs))
def __init__(self, is_training=True):
""" """
rospy.init_node('deepq_carnode')
self.rate = rospy.Rate(2) # send 2 observations per second
self.camera = picamera.PiCamera()
# self.camera.vflip = True
# self.camera.hflip = True
self.output = picamera.array.PiRGBArray(self.camera)
self.last_state = None
self.crashed = False
self.dead_counter = 0
self.observations = deque()
self.width = 320
self.height = 240
self.camera.resolution = (self.width, self.height)
self.publish_train = False
self.terminal_frame = False
self.trainer = rospy.Publisher('/pi_car/start_training',
Bool,
queue_size=1)
self.pub = rospy.Publisher('/pi_car/observation',
String,
queue_size=15,
latch=True)
rospy.Subscriber('/pi_car/cmd_resume', Bool, self.resume)
self._run = True
self._wait = False
self.driver = AStar()
self.previous_controls = (65, 65)
self.current_controls = (75, 35)
self.distance_from_router = 1.0
self.recovery_count = 0
self.reward_is_stuck = -.005
self.global_counter = 0
self.recovery_at_counter = 0
self._is_training = True
if not self._is_training:
self._tf._create_convolutional_network()
def start_search(event=None):
global start, goal, walls, map_creation, a_star
if map_creation.start == None or map_creation.goal == None:
map_creation.random_map()
start = map_creation.start
goal = map_creation.goal
walls = map_creation.walls
a_star = AStar(start, goal, R_C, R_C, walls)
t2 = threading.Thread(target=star_thread)
t2.daemon = True
t2.start()
draw_path()
def reset_algorithm(self):
"""Resets the algorithm"""
self.costmap_widget.canvas.freeze = True
self.costmap[:] = 0.0
Obstacle(3, 3, 3, 3).draw(self.costmap)
Obstacle(9, 5, 3, 3).draw(self.costmap)
Obstacle(16, 4, 3, 3).draw(self.costmap)
temp = self.costmap_widget.canvas.start_coord
self.start_coord = (floor(temp[0] + 0.5), floor(temp[1] + 0.5))
temp = self.costmap_widget.canvas.goal_coord
self.goal_coord = (floor(temp[0] + 0.5), floor(temp[1] + 0.5))
self.a_star = AStar(self.costmap, self.start_coord, self.goal_coord,
self.heuristic)
self.costmap_widget.canvas.freeze = False
self.costmap_widget.canvas.on_map_update()
def get_all_paths(map2d):
big_list=dict()
for x_start in range(0,12):
for y_start in range(0,12):
for x_end in range(0,12):
for y_end in range(0,12):
if x_start == x_end and y_start == y_end:
continue
elif map2d[x_start][y_start] == 1 or map2d[x_end][y_end] == 1:
continue
astar = AStar(map2d, Point(x_start, y_start), Point(x_end, y_end))
try:
path_list = [(p.y, p.x) for p in astar.start()]
except:
continue
big_list[(y_start, x_start, y_end, x_end)] = path_list
# print(list(big_list.keys())[10], big_list[list(big_list.keys())[10]])
return big_list
def go_home(map2d, x,y,home_x,home_y):
print(x, y)
astar = AStar(map2d, Point(y, x), Point(home_y, home_x))
path_list = [(p.y, p.x) for p in astar.start()]
print(path_list)
# action = "S"
if path_list[0][0]==x:
if path_list[0][1] > y:
action='R'
else:
action='L'
else:
if path_list[0][0] > x:
action='D'
else:
action='U'
return action
