def main(args):
# resolve path to world map definition
if not args.world:
world_map_path = os.path.join(
os.path.dirname(os.path.realpath(__file__)), 'world_map.txt')
else:
world_map_path = args.world
print("Reading world from %s" % world_map_path)
if not os.path.exists(world_map_path):
raise IOError(
"World map definition not found at its expected path: %s" %
world_map_path)
world = World(world_map_path)
visualizer = Visualizer(world)
# Value Iteration
value_iteration = ValueIteration(world,
one_step_cost_v1,
discount_factor=args.gamma,
eps=10e-10)
value_iteration.execute()
optimal_policy = value_iteration.extract_policy()
fig_vi = plt.figure()
visualizer.draw(fig_vi, optimal_policy, value_iteration.value_fn,
"Value Iteration (gamma = %.2f)" % args.gamma)
# Policy Iteration
policy_iteration = PolicyIteration(world,
one_step_cost_v1,
discount_factor=args.gamma)
value_fn = policy_iteration.execute()
fig_pi = plt.figure()
visualizer.draw(fig_pi, policy_iteration.policy, value_fn,
"Policy Iteration (gamma = %.2f)" % args.gamma)
plt.show()
source = map_instance.get_intersection(source_index)
print "Selected Source:", str(source)
dest_index = raw_input(
"\nEnter the index of the destination intersection. (Hit enter to choose the default value)"
)
if not dest_index:
dest_index = 76344
dest = map_instance.get_intersection(dest_index)
print "Destination:", str(dest)
algo_choice = raw_input(
"\nPlease choose the algorithm to run. \n1. A-Star Algorithm \n2. Dijkstra's Algorithm\n(Hit enter to choose the default algorithm)\n"
)
if (algo_choice == 1):
Algo = DijkstraShortestPathFinder
else:
Algo = AStarShortestPathFinder
print "Selected Algorithm:", Algo
print "\nExecuting.."
result = Algo(map_instance).get_shortest_path(source, dest)
print "Done. Printing the shortest path"
for i in result[1]:
print i
v = Visualizer()
v.draw(map_instance, result[1])
def run_test(arguments):
# Reset Tensorflow graph
tf.reset_default_graph()
# Instantiate publisher and subscriber for Gazebo
Interface.init()
traj = Trajectory(Interface)
visualizer = Visualizer(0.046 * 50)
refresh_rate = 1 / 20.
# Initialize policy network
policy_net = nn.NeuralNet(shape=Config.POLICY_SHAPE)
saver = tf.train.Saver()
with tf.Session() as sess:
sess.run(tf.global_variables_initializer())
if isinstance(arguments.test, str):
saver.restore(sess, arguments.test)
for _ in range(1):
# Generate random start position for the Quadcopter
traj.set_pose(Trajectory.random_pose())
refresh_time = 0
TARGETS = [[0, 0, 0], [0, 0, 5], [5, 5, 5], [5, -5,
5], [-5, -5, 5],
[-5, 5, 5], [5, 5, 5], [0, 0, 5], [0, 0, 0]]
c = 0
positions = []
for i in range(2500):
if i % 250 == 0:
TARGET = TARGETS[c]
c += 1
print("TARGET: ", TARGET)
# Get state information from drone subscriber
state = traj.get_state()
orientation = state[0:9]
position = state[9:12]
angular = state[12:15]
linear = state[15:18]
positions.append(position)
# Calculate rotation matrix from quaternion
orientation = Quaternion(
matrix=np.reshape(orientation, (3, 3)))
state = {
'position': position,
'rotation_matrix': orientation.rotation_matrix
}
if refresh_time - refresh_rate < time():
visualizer.update(state, 0)
visualizer.draw(positions)
visualizer.draw_target(TARGETS)
refresh_time = time()
orientation = np.ndarray.flatten(orientation.rotation_matrix)
# Calculate relative distance to target position
#position = np.subtract(position, TARGET)
position = np.subtract(position,
[-0.29748929, -0.66516153, -0.43737027])
position = np.subtract(position, TARGET)
# Concatenate all to generate an input state vector for the networks
state = np.concatenate(
(orientation, position, angular, linear))
# Predict action with policy network
action = Utils.forward(sess, policy_net, [state])[0]
action = Config.ACTION_SCALE * action
# Feed action vector to the drone
traj.step(action)
input('Press Enter to exit')
flights_len += len(flights[ky])
print("---------------------------------------------")
print("Some Statistics:")
print("---------------------------------------------")
print("Total airports in the dataset:", len(airports))
print("Total flight segments in the dataset:", flights_len)
print("Total customers in the dataset:", len(customers))
print("Total trips in the dataset:", len(trips))
print("---------------------------------------------\n")
all_flights = [seg for tp in trips for seg in tp.get_flight_segments()]
all_customers = [customers[cid] for cid in customers]
V = Visualizer()
V.draw(all_flights)
while not V.has_quit():
flights = V.handle_window_events(all_customers, all_flights)
all_flights = []
for flt in flights:
all_flights.append(flt)
V.draw(all_flights)
import python_ta
python_ta.check_all(
config={