def plan_to_goal(self, start, goal):
"""
Plan a path from start to goal
Return a path
"""
# Implement here
# Plan with lazy_astar
self.time_left= self.total_planning_time
start, goal = tuple(start), tuple(goal)
start_time = time.time()
self.G, _ = graph_maker.add_node(self.G, start, env=self.planning_env,
connection_radius=self.connection_radius)
self.G, _ = graph_maker.add_node(self.G, goal, env=self.planning_env,
connection_radius=self.connection_radius)
print("t2 = ", rospy.get_time() - start_time)
rospy.loginfo('planning from start to goal begin time: {}'.format(time.time()))
start_time =rospy.get_time()
try:
path_nodes, dist = lazy_astar.astar_path(self.G, source=start, target=goal,
weight=self.weight_func, heuristic=self.heuristic_func, return_dist=self.plan_with_budget)
except NetworkXNoPath:
return None
print "planning time = ", rospy.get_time() - start_time
rospy.loginfo('done time: {}'.format(rospy.get_time()))
start_time = self.tic_toc(rospy.get_time()- start_time)
idx = 0
max_sample_num = 10
added_nodes = []
while self.plan_with_budget and self.time_left > 0:
added_node, ellipse, start_time = self.densify_graph(start, goal, dist, max_sample_num)
added_nodes.extend(added_node)
try:
path_nodes, dist = lazy_astar.astar_path(self.G, source=start, target=goal,
weight=self.weight_func, heuristic=self.heuristic_func, return_dist=True)
except NetworkXNoPath:
# avoid replacing the old path
IPython.embed()
self.remove_nodes(added_nodes)
added_nodes = []
continue
self.path_nodes = path_nodes
self.visualiza_plan = True
start_time = self.tic_toc(time.time() - start_time)
idx += 1
# self.planning_env.visualize_graph(self.G, start, goal, added_nodes, ellipse, self.path_nodes, 'densify_{}'.format(idx))
print 'current min distance: {}, added node: {}'.format(dist, len(added_nodes))
print 'planning time left', self.time_left
rospy.loginfo('done planning.')
self.path_nodes = path_nodes
self.visualiza_plan = False
print('path length before shortcut: {}'.format(dist))
if self.do_shortcut:
path_nodes = self.planning_env.shortcut(self.G, path_nodes)
path = self.planning_env.get_path_on_graph(self.G, path_nodes)
self.remove_nodes(added_nodes)
return path
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
"""
# Implement here
# Plan with lazy_astar
path = lazy_astar.astar_path(G,
source=start,
target=goal,
weight=self.weight_func,
heuristic=self.heuristic_func)
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
#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(
# G.nodes[n1]['config'], G.nodes[n2]['config'])
# n1, n2)
end = time.time()
if args.lazy:
print("lazy A*")
weight = planning_env.edge_validity_checker
#weight = lambda n1, n2: planning_env.edge_validity_checker(n1, n2)
# G.nodes[n1]['config'], G.nodes[n2]['config'])
<<<<<<< HEAD
path, dist = lazy_astar.astar_path(G,
source=start_id, target=goal_id, weight=weight, heuristic=heuristic)
else:
path, dist = astar.astar_path(G,
source=start_id, target=goal_id, heuristic=heuristic)
print('plan time: ', time.time() - end)
# print('path length: {}'.format(dist))
#print("path we got", path)
if args.shortcut:
end = time.time()
plan = planning_env.shortcut(G, path)
print('short cut time: ', time.time() - end)
print(path)
dist = planning_env.compute_path_length(path)
print('path length: {}'.format(dist))
=======
path = lazy_astar.astar_path(G,
def plan_to_goal(self, start, goal):
"""
Plan a path from start to goal
Return a path
"""
# Implement here
# Plan with lazy_astar
self.time_left = self.total_planning_time
start, goal = tuple(start), tuple(goal)
start_time = time.time()
self.G, _ = graph_maker.add_node(
self.G,
start,
env=self.planning_env,
connection_radius=self.connection_radius)
self.G, _ = graph_maker.add_node(
self.G,
goal,
env=self.planning_env,
connection_radius=self.connection_radius)
print("t2 = ", time.time() - start_time)
rospy.loginfo('planning from start to goal...')
start_time = time.time()
path_nodes, dist = lazy_astar.astar_path(
self.G,
source=start,
target=goal,
weight=self.weight_func,
heuristic=self.heuristic_func,
return_dist=self.plan_with_budget)
print "planning time = ", time.time() - start_time
start_time = self.tic_toc(time.time() - start_time)
idx = 0
max_sample_num = 16
added_nodes = []
while self.plan_with_budget and self.time_left > 0:
added_node, ellipse, start_time = self.densify_graph(
start, goal, dist, max_sample_num)
added_nodes.extend(added_node)
path_nodes, dist = lazy_astar.astar_path(
self.G,
source=start,
target=goal,
weight=self.weight_func,
heuristic=self.heuristic_func,
return_dist=True)
self.path_nodes = path_nodes
self.visualiza_plan = True
start_time = self.tic_toc(time.time() - start_time)
idx += 1
self.planning_env.visualize_graph(self.G, start, goal, added_nodes,
ellipse, self.path_nodes,
'densify_{}'.format(idx))
print 'current minimum distance', dist
print 'total connect nodes', len(added_nodes)
print 'planning time left', self.time_left
rospy.loginfo('done planning.')
self.path_nodes = path_nodes
self.visualiza_plan = False
print('path length before shortcut: {}'.format(dist))
if self.do_shortcut:
path_nodes = self.planning_env.shortcut(self.G, path_nodes)
path = self.planning_env.get_path_on_graph(self.G, path_nodes)
return path