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