def navigate(self, dest):
"""Find a path from current location to the given destination."""
# Make our node objects for A*.
rospy.loginfo("POSE: %f, %f" %(self.pose.x, self.pose.y))
start = Landmark(name="START", ltype="END", x=self.pose.x, y=self.pose.y)
goal = Landmark(name="GOAL", ltype="END", x=dest.x, y=dest.y)
# Find visible neighbors of start and goal to add as edges to the graph.
landmarks = imap(lambda pair: pair[0], self.landmark_graph.itervalues())
# Here we manually add the goal to the candidate landmarks for start
# to allow for navigation directly to the goal location.
start_zone = self.find_nearest_visibles(start, chain(landmarks, [goal]))
# We have to add start here for the empty zone check below.
landmarks = imap(lambda pair: pair[0], self.landmark_graph.itervalues())
goal_zone = self.find_nearest_visibles(goal, chain(landmarks, [start]))
rospy.loginfo("Start zone: %s" % (", ".join(l.name for l in start_zone)))
rospy.loginfo("Goal zone: %s" % (", ".join(l.name for l in goal_zone)))
if not start_zone or not goal_zone:
rospy.logerr("Cannot connect start and goal! A* failed.")
return [];
# A* functions.
is_goal = lambda node: node.name == "GOAL"
heuristic = lambda node: point_distance(node, goal)
def neighbors(node):
if node.name == "START":
return start_zone
nbrs = self.landmark_graph[node.name][1]
if node in goal_zone:
# Intentionally use list concat to make a copy of the list.
# If we just modify nbrs, it will modify the original graph.
return nbrs + [goal]
return nbrs
return a_star(start, is_goal, neighbors, point_distance, heuristic)
def find_nearest_visibles(self, point, landmarks):
"""Finds landmarks visible from a point.
Limited to only nodes within MAX_ZONE_DIST of point, unless there
are none in which case the single next closest node is returned.
Arguments:
point - The starting point.
landmarks - An iter of landmarks to consider.
Returns a list of up to MAX_ZONE_SIZE closest visible Landmarks.
"""
#rospy.loginfo("Calculating visibles for %s", point)
#rospy.loginfo("Selecting from %s" % (", ".join(l.name for l in landmarks)))
visibles = []
for landmark in landmarks:
if self.navigable(point, landmark) and landmark.ltype in [
'NSH', 'I', 'WEH', 'END'
]:
d = point_distance(point, landmark)
visibles.append((d, landmark))
visibles.sort()
for d, landmark in visibles:
rospy.loginfo("%s at %f", landmark.name, d)
nearest = []
for d, landmark in visibles[:CorobotNavigator.MAX_ZONE_SIZE]:
# Limit to within MAX_ZONE_DIST, but take at least one.
if d < CorobotNavigator.MAX_ZONE_DIST or not nearest:
nearest.append(landmark)
return nearest
def find_nearest_visibles(self, point, landmarks):
"""Finds landmarks visible from a point.
Limited to only nodes within MAX_ZONE_DIST of point, unless there
are none in which case the single next closest node is returned.
Arguments:
point - The starting point.
landmarks - An iter of landmarks to consider.
Returns a list of up to MAX_ZONE_SIZE closest visible Landmarks.
"""
#rospy.loginfo("Calculating visibles for %s", point)
#rospy.loginfo("Selecting from %s" % (", ".join(l.name for l in landmarks)))
visibles = []
for landmark in landmarks:
if self.navigable(point, landmark) and landmark.ltype in ['NSH', 'I', 'WEH', 'END']:
d = point_distance(point, landmark)
visibles.append((d, landmark))
visibles.sort()
for d, landmark in visibles:
rospy.loginfo("%s at %f", landmark.name, d)
nearest = []
for d, landmark in visibles[:CorobotNavigator.MAX_ZONE_SIZE]:
# Limit to within MAX_ZONE_DIST, but take at least one.
if d < CorobotNavigator.MAX_ZONE_DIST or not nearest:
nearest.append(landmark)
return nearest
def navigate(self, dest):
"""Find a path from current location to the given destination."""
# Make our node objects for A*.
start = Landmark(name="START", x=self.pose.x, y=self.pose.y)
goal = Landmark(name="GOAL", x=dest.x, y=dest.y)
# Find visible neighbors of start and goal to add as edges to the graph.
landmarks = imap(lambda pair: pair[0], self.landmark_graph.itervalues())
# Here we manually add the goal to the candidate landmarks for start
# to allow for navigation directly to the goal location.
start_zone = self.find_nearest_visibles(start, chain(landmarks, [goal]))
# We have to add start here for the empty zone check below.
landmarks = imap(lambda pair: pair[0], self.landmark_graph.itervalues())
goal_zone = self.find_nearest_visibles(goal, chain(landmarks, [start]))
rospy.loginfo("Start zone: %s" % (", ".join(l.name for l in start_zone)))
rospy.loginfo("Goal zone: %s" % (", ".join(l.name for l in goal_zone)))
if not start_zone or not goal_zone:
rospy.logerr("Cannot connect start and goal! A* failed.")
return []
# A* functions.
is_goal = lambda node: node.name == "GOAL"
heuristic = lambda node: point_distance(node, goal)
def neighbors(node):
if node.name == "START":
return start_zone
nbrs = self.landmark_graph[node.name][1]
if node in goal_zone:
# Intentionally use list concat to make a copy of the list.
# If we just modify nbrs, it will modify the original graph.
return nbrs + [goal]
return nbrs
return a_star(start, is_goal, neighbors, point_distance, heuristic)