def planner(self, req):
""" Plan route from start to goal.
:param req: `geographic_msgs/GetRoutePlan`_ request message.
:returns: `geographic_msgs/RoutePath`_ message.
:raises: :exc:`ValueError` if invalid request.
:raises: :exc:`NoPathToGoalError` if goal not reachable.
"""
# validate request parameters
if req.network.uuid != self.graph.id.uuid: # different route network?
raise ValueError('invalid GetRoutePlan network: '
+ str(req.network.uuid))
start_idx = self.points.index(req.start.uuid)
if start_idx is None:
raise ValueError('unknown starting point: '
+ str(req.start.uuid))
goal_idx = self.points.index(req.goal.uuid)
if goal_idx is None:
raise ValueError('unknown goal: ' + str(req.goal.uuid))
# initialize plan
plan = RoutePath(network=self.graph.id)
plan.network = req.network
# A* shortest path algorithm
opened = [[0.0, start_idx]]
closed = [False for wid in xrange(len(self.points))]
closed[start_idx] = True
backpath = [None for wid in xrange(len(self.points))]
while True:
if len(opened) == 0:
raise NoPathToGoalError('No path from ' + req.start.uuid
+ ' to ' + req.goal.uuid)
opened.sort() # :todo: make search more efficient
opened.reverse()
h, e = opened.pop()
if e == goal_idx:
break
for edge in self.edges[e]:
e2 = edge.end
if not closed[e2]:
h2 = h + edge.h
opened.append([h2, e2])
closed[e2] = True
backpath[e2] = [e, edge]
# generate plan segments from backpath
plan.segments = []
e = goal_idx
while backpath[e] is not None:
plan.segments.append(backpath[e][1].seg)
e = backpath[e][0]
assert(e == start_idx)
plan.segments.reverse()
return plan
def planner(self, req):
""" Plan route from start to goal.
:param req: `geographic_msgs/GetRoutePlan`_ request message.
:returns: `geographic_msgs/RoutePath`_ message.
:raises: :exc:`ValueError` if invalid request.
:raises: :exc:`NoPathToGoalError` if goal not reachable.
"""
# validate request parameters
if req.network.uuid != self.graph.id.uuid: # different route network?
raise ValueError('invalid GetRoutePlan network: '
+ str(req.network.uuid))
start_idx = self.points.index(req.start.uuid)
if start_idx is None:
raise ValueError('unknown starting point: '
+ str(req.start.uuid))
goal_idx = self.points.index(req.goal.uuid)
if goal_idx is None:
raise ValueError('unknown goal: ' + str(req.goal.uuid))
# initialize plan
plan = RoutePath(network=self.graph.id)
plan.network = req.network
# A* shortest path algorithm
opened = [[0.0, start_idx]]
closed = [False for wid in xrange(len(self.points))]
closed[start_idx] = True
backpath = [None for wid in xrange(len(self.points))]
while True:
if len(opened) == 0:
raise NoPathToGoalError('No path from ' + req.start.uuid
+ ' to ' + req.goal.uuid)
opened.sort() # :todo: make search more efficient
opened.reverse()
h, e = opened.pop()
if e == goal_idx:
break
for edge in self.edges[e]:
e2 = edge.end
if not closed[e2]:
h2 = h + edge.h
opened.append([h2, e2])
closed[e2] = True
backpath[e2] = [e, edge]
# generate plan segments from backpath
plan.segments = []
e = goal_idx
while backpath[e] is not None:
plan.segments.append(backpath[e][1].seg)
e = backpath[e][0]
assert(e == start_idx)
plan.segments.reverse()
return plan
def _planner_seg(self, start_geo_point, start_seg, goal_geo_point, goal_seg):
""" Plan route from start to goal. The actual search algorithm to find a path is executed here.
:param start_geo_point: The start position.
:type start_geo_point: geographic_msgs/GeoPoint
:param start_seg: The nearest segment to the point.
:type start_seg: geographic_msgs/RouteSegment
:param: goal_geo_point: The goal position.
:type goal_geo_point: geographic_msgs/GeoPoint
:param goal_seg: The nearest segment to the point.
:type goal_seg: geographic_msgs/RouteSegment
:return: The planned path between start and goal, and its length.
:rtype: (geographic_msgs/RoutePath, float)
:raises: :exc: ValueError if invalid request.
:raises: :exc: NoPathToGoalError if goal not reachable.
"""
# validate request parameters
start__seg_start_idx = self.points.index(start_seg.start.uuid)
start__seg_end_idx = self.points.index(start_seg.end.uuid)
if start__seg_start_idx is None or start__seg_end_idx is None:
raise ValueError('unknown starting segment: '
+ str(start_seg.id))
goal__seg_start_idx = self.points.index(goal_seg.start.uuid)
goal__seg_end_idx = self.points.index(goal_seg.end.uuid)
if goal__seg_start_idx is None or goal__seg_end_idx is None:
raise ValueError('unknown goal segment: ' + str(goal_seg.id))
# initialize plan
plan = RoutePath(network=self.graph.id)
# A* shortest path algorithm
opened = []
# add the distance from start point to the start and to the end of the nearest segment
start_utm = geodesy.utm.fromMsg(start_geo_point)
utm_seg_start = self.utm_points[self.points.index(start_seg.start.uuid)]
utm_seg_end = self.utm_points[self.points.index(start_seg.end.uuid)]
length_start2seg_start = self.distance2D(start_utm, utm_seg_start)
length_start2seg_end = self.distance2D(start_utm, utm_seg_end)
# distance of the last segment to the goal
goal_utm = geodesy.utm.fromMsg(goal_geo_point)
utm_goal_seg_start = self.utm_points[self.points.index(goal_seg.start.uuid)]
utm_goal_seg_end = self.utm_points[self.points.index(goal_seg.end.uuid)]
length_goal2seg_start = self.distance2D(goal_utm, utm_goal_seg_start)
length_goal2seg_end = self.distance2D(goal_utm, utm_goal_seg_end)
opened.append([length_start2seg_start, start__seg_start_idx])
closed = [0 for wid in xrange(len(self.points))]
closed[start__seg_start_idx] = -1.
backpath = [None for wid in xrange(len(self.points))]
reached_goal = None
while True:
if len(opened) == 0:
raise NoPathToGoalError('No path from ' + str(start_geo_point) + ' to ' + str(goal_geo_point))
opened.sort()
h, e = opened.pop(0)
if ((e == goal__seg_start_idx and goal__seg_start_idx != start__seg_start_idx) or (e == goal__seg_end_idx and goal__seg_end_idx != start__seg_start_idx)):
reached_goal = e
break
for edge in self.edges[e]:
e2 = edge.end
if e2 == start__seg_end_idx:
h2 = length_start2seg_end
elif e2 == start__seg_start_idx:
h2 = length_start2seg_start
else:
h2 = h + edge.h
if e2 == goal__seg_start_idx:
h2 += length_goal2seg_start
elif e2 == goal__seg_end_idx:
h2 += length_goal2seg_end
if closed[e2] == 0 or closed[e2] > h2:
opened.append([h2, e2])
closed[e2] = h2
backpath[e2] = [e, edge, h2]
# generate plan segments from backpath
plan.segments = []
if reached_goal is None:
raise ValueError('no path to target found')
e = reached_goal
dist = backpath[e][2] if backpath[e] is not None else -1
try:
while backpath[e] is not None:
plan.segments.append(backpath[e][1].seg)
e = backpath[e][0]
# :TODO: sometimeswe we have an MemoryError
except:
print "Error, count of segments: ", len(plan.segments)
raise
assert(e == start__seg_start_idx or e == start__seg_end_idx)
plan.segments.reverse()
return plan, dist
def _planner_seg(self, start_geo_point, start_seg, goal_geo_point, goal_seg):
""" Plan route from start to goal.
:param start_geo_point: `geographic_msgs/GeoPoint`
:param start_seg: `geographic_msgs/RouteSegment`, the nearest segment to the point.
:param goal_geo_point: `geographic_msgs/GeoPoint`
:param goal_seg: `geographic_msgs/RouteSegment`, the nearest segment to the point.
:returns: a tuple of `geographic_msgs/RoutePath`_ message and distance
:raises: :exc:`ValueError` if invalid request.
:raises: :exc:`NoPathToGoalError` if goal not reachable.
"""
# validate request parameters
start__seg_start_idx = self.points.index(start_seg.start.uuid)
start__seg_end_idx = self.points.index(start_seg.end.uuid)
if start__seg_start_idx is None or start__seg_end_idx is None:
raise ValueError('unknown starting segment: '
+ str(start_seg.id))
goal__seg_start_idx = self.points.index(goal_seg.start.uuid)
goal__seg_end_idx = self.points.index(goal_seg.end.uuid)
if goal__seg_start_idx is None or goal__seg_end_idx is None:
raise ValueError('unknown goal segment: ' + str(goal_seg.id))
# initialize plan
plan = RoutePath(network=self.graph.id)
# A* shortest path algorithm
opened = []
# add the distance from start point to the start and to the end of the nearest segment
start_utm = geodesy.utm.fromMsg(start_geo_point)
utm_seg_start = self.utm_points[self.points.index(start_seg.start.uuid)]
utm_seg_end = self.utm_points[self.points.index(start_seg.end.uuid)]
length_start2seg_start = self.distance2D(start_utm, utm_seg_start)
length_start2seg_end = self.distance2D(start_utm, utm_seg_end)
# distance of the last segment to the goal
goal_utm = geodesy.utm.fromMsg(goal_geo_point)
utm_goal_seg_start = self.utm_points[self.points.index(goal_seg.start.uuid)]
utm_goal_seg_end = self.utm_points[self.points.index(goal_seg.end.uuid)]
length_goal2seg_start = self.distance2D(goal_utm, utm_goal_seg_start)
length_goal2seg_end = self.distance2D(goal_utm, utm_goal_seg_end)
opened.append([length_start2seg_start, start__seg_start_idx])
closed = [0 for wid in xrange(len(self.points))]
closed[start__seg_start_idx] = -1.
backpath = [None for wid in xrange(len(self.points))]
reached_goal = None
while True:
if len(opened) == 0:
raise NoPathToGoalError('No path from ' + str(start_geo_point)
+ ' to ' + str(goal_geo_point))
opened.sort() # :todo: make search more efficient
opened.reverse()
h, e = opened.pop()
if ((e == goal__seg_start_idx and goal__seg_start_idx != start__seg_start_idx)
or (e == goal__seg_end_idx and goal__seg_end_idx != start__seg_start_idx)):
reached_goal = e
break
for edge in self.edges[e]:
e2 = edge.end
if e2 == start__seg_end_idx:
h2 = length_start2seg_end
elif e2 == start__seg_start_idx:
h2 = length_start2seg_start
else:
h2 = h + edge.h
if e2 == goal__seg_start_idx:
h2 += length_goal2seg_start
elif e2 == goal__seg_end_idx:
h2 += length_goal2seg_end
if closed[e2] == 0 or closed[e2] > h2:
opened.append([h2, e2])
closed[e2] = h2
backpath[e2] = [e, edge, h2]
# generate plan segments from backpath
plan.segments = []
if reached_goal is None:
raise ValueError('no path to target found')
e = reached_goal
dist = backpath[e][2] if backpath[e] is not None else -1
try:
while backpath[e] is not None:
plan.segments.append(backpath[e][1].seg)
e = backpath[e][0]
# todo sometimeswe we have an MemoryError
except:
print "Error, count of segments: ", len(plan.segments)
raise
assert(e == start__seg_start_idx or e == start__seg_end_idx)
plan.segments.reverse()
# print "Distance", dist
return plan, dist
