def plan_to_goal(self, start, goal):
"""
Plan a path from start to goal
Return a path
"""
# TODO
# Plan with lazy_astar
self.G, start_id = graph_maker.add_node(
self.G,
start,
env=self.planning_env,
connection_radius=self.connection_radius,
start_from_config=True)
self.G, goal_id = graph_maker.add_node(
self.G,
goal,
env=self.planning_env,
connection_radius=self.connection_radius,
start_from_config=False)
path_nodes = lazy_astar.astar_path(self.G,
source=start_id,
target=goal_id,
weight=self.weight_func,
heuristic=self.heuristic_func)
if self.do_shortcut:
self.planning_env.shortcut(self.G, path_nodes)
path = self.planning_env.get_path_on_graph(self.G, path_nodes)
path = self.map2world(path)
return path
def plan_to_goal(self, start, goal):
"""
Plan a path from start to goal
Return a path in world frame.
"""
# TODO: Implement here
# 1. Add start and goal nodes to the graph
# 2. Call the lazy a* planner
# 3. Do shortcut if necessary
self.G, start_id = graph_maker.add_node(
self.G,
start,
env=self.planning_env,
connection_radius=self.connection_radius,
start_from_config=True,
lazy=True)
self.G, goal_id = graph_maker.add_node(
self.G,
goal,
env=self.planning_env,
connection_radius=self.connection_radius,
start_from_config=False,
lazy=True)
lastar_start = time.time()
path = lazy_astar.astar_path(self.G,
source=start_id,
target=goal_id,
weight=self.weight_func,
heuristic=self.heuristic_func)
lastar_end = time.time()
print("Lazy planning time: ", lastar_end - lastar_start)
#self.planning_env.visualize_plan(self.G,path)
lastar_end = time.time()
if (len(path) > 2):
path = self.planning_env.shortcut(self.G, path)
print("Shortcut time: ", time.time() - lastar_end)
#self.planning_env.visualize_plan(self.G,path)
# Convert the generated map path to world path.
# configs is the list of waypoints on the path.
configs = self.planning_env.get_path_on_graph(self.G, path)
world_points = util.map_to_world(configs, self.map.info)
return world_points
<<<<<<< HEAD
end = time.time()
=======
>>>>>>> a7e57bfd30cce53a52a585e8e419a8029de5343d
# Make a graph
G = graph_maker.make_graph(planning_env,
sampler=Sampler(planning_env),
num_vertices=args.num_vertices,
connection_radius=args.connection_radius,
lazy=args.lazy)
# Add start and goal nodes
print(args.start, args.goal)
G, start_id = graph_maker.add_node(G, args.start, env=planning_env,
connection_radius=args.connection_radius)
G, goal_id = graph_maker.add_node(G, args.goal, env=planning_env,
connection_radius=args.connection_radius)
<<<<<<< HEAD
print('graph making time: ', time.time() - end)
# Uncomment this to visualize the graph
# planning_env.visualize_graph(G, tuple(args.start), tuple(args.goal))
=======
# Uncomment this to visualize the graph
#planning_env.visualize_graph(G, tuple(args.start), tuple(args.goal))
>>>>>>> a7e57bfd30cce53a52a585e8e419a8029de5343d
try:
heuristic = planning_env.compute_heuristic
#heuristic = lambda n1, n2: planning_env.compute_heuristic(
map_data = np.loadtxt(args.map).astype(np.int)
planning_env = MapEnvironment(map_data)
for i in range(9):
# Make a graph
G = graph_maker.make_graph(planning_env,
sampler=Sampler(planning_env),
num_vertices=args.num_vertices,
connection_radius=args.connection_radius,
directed = False,
lazy=args.lazy,
saveto = 'graph(%d).pkl' % i )
print("made graph")
# Add start and goal nodes
G, start_id = graph_maker.add_node(G, args.start, env=planning_env,
connection_radius=args.connection_radius, lazy = False, start_from_config=True)
G, goal_id = graph_maker.add_node(G, args.goal, env=planning_env,
connection_radius=args.connection_radius, lazy = False, start_from_config=False)
# Uncomment this to visualize the graph
# planning_env.visualize_graph(G)
try:
heuristic = lambda n1, n2: planning_env.compute_heuristic(
G.nodes[n1]['config'], G.nodes[n2]['config'])
if args.lazy:
weight = lambda n1, n2: planning_env.edge_validity_checker(
G.nodes[n1]['config'], G.nodes[n2]['config'])