unit6_pp/scripts/planner_solution.py
def geo_path(self, req):
        """ Plan the shortest path between a start and a destination geopoint.

        The 'geo_path' method can receive GeoPoints out of the graph, upon such a case, the nearest segments on the OSM map are detected,
        and the planning is carried out.
        
        :pram req: The request message.
        :param req: geographic_msgs/GetGeoPath

        :return: The computed path, as well as the ids of the RouteNetwork and the start and end segments, plus the length of the path. The length is set to -1 in case of failure.
        :rtype: (geographic_msgs/GeoPoint[], uuid_msgs/UniqueID, uuid_msgs/UniqueID, uuid_msgs/UniqueID, length) 
        :raises: :exc:`ValueError` if invalid request.
        """
        # check for possible errors in request
        if math.isnan(req.start.latitude) or math.isnan(req.start.longitude):
            raise ValueError('NaN in starting point: ' + str(req.start))
        if math.isnan(req.goal.latitude) or math.isnan(req.goal.longitude):
            raise ValueError('NaN in starting point: ' + str(req.start))
        if math.isnan(req.start.altitude):
            req.start.altitude = 0.0
        if math.isnan(req.goal.altitude):
            req.goal.altitude = 0.0
        # get the nearest segment to the start point
        (start_seg, start_dist, start_lot) = self.getNearestSegment(req.start)
        if start_seg is None:
            raise ValueError('no nearest segment found for starting way point: ' + str(req.start))
        # get the nearest segment to the destination point
        (goal_seg, goal_dist, goal_lot) = self.getNearestSegment(req.goal)
        if goal_seg is None:
            raise ValueError('no nearest segment found for goal way point: ' + str(req.goal))

        # determine the path
        result = []
        try:
            route_path = self.dijkstra_geo_planner(start_seg, goal_seg)

        except NoPathToGoalError as e:
            return result, self.graph.id, start_seg.id, goal_seg.id, -1

        start_utm = geodesy.utm.fromMsg(req.start)
        goal_utm = geodesy.utm.fromMsg(req.goal)
        # copy the route to the result list and adds the start and/or goal position to the path
        if route_path.segments:
            seg = self._getSegment(route_path.segments[0])
            p = self._get_min_point(start_seg, start_lot)
            # add the start point if it is not close to the route
            dist_first_to_start = self.distance2D(p, start_utm)
            if dist_first_to_start > self._shift_to_route_within:
                result.append(req.start)
            result.append(p.toMsg())
            # add only the endpoints of the segments
            for index in xrange(len(route_path.segments)):
                seg_id = route_path.segments[index]
                seg = self._getSegment(seg_id)

                if index == 0:
                    # add the segment start point, if the plan segment and nearest segment are not equal
                    if not ((seg.end.uuid == start_seg.start.uuid and seg.start.uuid == start_seg.end.uuid) or (seg.start.uuid == start_seg.start.uuid and seg.end.uuid == start_seg.end.uuid)):
                        result.append(self.points[seg.start.uuid].position())
                if index+1 == len(route_path.segments):
                    # add the end point of the last segment, if the plan segment and nearest segment are not equal
                    if not ((seg.end.uuid == goal_seg.start.uuid and seg.start.uuid == goal_seg.end.uuid) or (seg.start.uuid == goal_seg.start.uuid and seg.end.uuid == goal_seg.end.uuid)):
                        result.append(self.points[seg.end.uuid].position())
                else:
                    result.append(self.points[seg.end.uuid].position())
            # add a perpendicular point or the nearest endpoint of the end segment
            p = self._get_min_point(goal_seg, goal_lot)
            result.append(p.toMsg())
            # add the destination point if it is not close to the route
            dist_last_to_goal = self.distance2D(p, goal_utm)
            if dist_last_to_goal > self._shift_to_route_within:
                result.append(req.goal)
        else:
            # direct connection
            result.append(req.start)
            result.append(req.goal)

        # calculate the distance
        last_utm = None
        dist = 0
        for point in result:
            if last_utm is None:
                last_utm = geodesy.utm.fromMsg(point)
            else:
                new_utm = geodesy.utm.fromMsg(point)
                dist += self.distance2D(last_utm, new_utm)
                last_utm = new_utm
        return result, self.graph.id, start_seg.id, goal_seg.id, dist
 Plan the shortest path between a start and a destination geopoint.

        The 'geo_path' method can receive GeoPoints out of the graph, upon such a case, the nearest segments on the OSM map are detected,
        and the planning is carried out.
        
        :pram req: The request message.
        :param req: geographic_msgs/GetGeoPath

        :return: The computed path, as well as the ids of the RouteNetwork and the start and end segments, plus the length of the path. The length is set to -1 in case of failure.
        :rtype: (geographic_msgs/GeoPoint[], uuid_msgs/UniqueID, uuid_msgs/UniqueID, uuid_msgs/UniqueID, length) 
        :raises: :exc:`ValueError` if invalid request.
        
Plan the shortest path between a start and a destination geopoint.
The 'geo_path' method can receive GeoPoints out of the graph, upon such a case, the nearest segments on the OSM map are detected,
and the planning is carried out.
Plan the shortest path between a start and a destination geopoint . The ' geo_path ' method can receive GeoPoints out of the graph upon such a case the nearest segments on the OSM map are detected and the planning is carried out .
def geo_path(self, req):
        if math.isnan(req.start.latitude) or math.isnan(req.start.longitude):
            raise ValueError('NaN in starting point: ' + str(req.start))
        if math.isnan(req.goal.latitude) or math.isnan(req.goal.longitude):
            raise ValueError('NaN in starting point: ' + str(req.start))
        if math.isnan(req.start.altitude):
            req.start.altitude = 0.0
        if math.isnan(req.goal.altitude):
            req.goal.altitude = 0.0
        (start_seg, start_dist, start_lot) = self.getNearestSegment(req.start)
        if start_seg is None:
            raise ValueError('no nearest segment found for starting way point: ' + str(req.start))
        (goal_seg, goal_dist, goal_lot) = self.getNearestSegment(req.goal)
        if goal_seg is None:
            raise ValueError('no nearest segment found for goal way point: ' + str(req.goal))
        result = []
        try:
            route_path = self.dijkstra_geo_planner(start_seg, goal_seg)
        except NoPathToGoalError as e:
            return result, self.graph.id, start_seg.id, goal_seg.id, -1
        start_utm = geodesy.utm.fromMsg(req.start)
        goal_utm = geodesy.utm.fromMsg(req.goal)
        if route_path.segments:
            seg = self._getSegment(route_path.segments[0])
            p = self._get_min_point(start_seg, start_lot)
            dist_first_to_start = self.distance2D(p, start_utm)
            if dist_first_to_start > self._shift_to_route_within:
                result.append(req.start)
            result.append(p.toMsg())
            for index in xrange(len(route_path.segments)):
                seg_id = route_path.segments[index]
                seg = self._getSegment(seg_id)
                if index == 0:
                    if not ((seg.end.uuid == start_seg.start.uuid and seg.start.uuid == start_seg.end.uuid) or (seg.start.uuid == start_seg.start.uuid and seg.end.uuid == start_seg.end.uuid)):
                        result.append(self.points[seg.start.uuid].position())
                if index+1 == len(route_path.segments):
                    if not ((seg.end.uuid == goal_seg.start.uuid and seg.start.uuid == goal_seg.end.uuid) or (seg.start.uuid == goal_seg.start.uuid and seg.end.uuid == goal_seg.end.uuid)):
                        result.append(self.points[seg.end.uuid].position())
                else:
                    result.append(self.points[seg.end.uuid].position())
            p = self._get_min_point(goal_seg, goal_lot)
            result.append(p.toMsg())
            dist_last_to_goal = self.distance2D(p, goal_utm)
            if dist_last_to_goal > self._shift_to_route_within:
                result.append(req.goal)
        else:
            result.append(req.start)
            result.append(req.goal)
        last_utm = None
        dist = 0
        for point in result:
            if last_utm is None:
                last_utm = geodesy.utm.fromMsg(point)
            else:
                new_utm = geodesy.utm.fromMsg(point)
                dist += self.distance2D(last_utm, new_utm)
                last_utm = new_utm
        return result, self.graph.id, start_seg.id, goal_seg.id, dist
def geo_path ( self , req ) : if math . isnan ( req . start . latitude ) or math . isnan ( req . start . longitude ) : raise ValueError ( 'NaN in starting point: ' + str ( req . start ) ) if math . isnan ( req . goal . latitude ) or math . isnan ( req . goal . longitude ) : raise ValueError ( 'NaN in starting point: ' + str ( req . start ) ) if math . isnan ( req . start . altitude ) : req . start . altitude = 0.0 if math . isnan ( req . goal . altitude ) : req . goal . altitude = 0.0 ( start_seg , start_dist , start_lot ) = self . getNearestSegment ( req . start ) if start_seg is None : raise ValueError ( 'no nearest segment found for starting way point: ' + str ( req . start ) ) ( goal_seg , goal_dist , goal_lot ) = self . getNearestSegment ( req . goal ) if goal_seg is None : raise ValueError ( 'no nearest segment found for goal way point: ' + str ( req . goal ) ) result = [ ] try : route_path = self . dijkstra_geo_planner ( start_seg , goal_seg ) except NoPathToGoalError as e : return result , self . graph . id , start_seg . id , goal_seg . id , - 1 start_utm = geodesy . utm . fromMsg ( req . start ) goal_utm = geodesy . utm . fromMsg ( req . goal ) if route_path . segments : seg = self . _getSegment ( route_path . segments [ 0 ] ) p = self . _get_min_point ( start_seg , start_lot ) dist_first_to_start = self . distance2D ( p , start_utm ) if dist_first_to_start > self . _shift_to_route_within : result . append ( req . start ) result . append ( p . toMsg ( ) ) for index in xrange ( len ( route_path . segments ) ) : seg_id = route_path . segments [ index ] seg = self . _getSegment ( seg_id ) if index == 0 : if not ( ( seg . end . uuid == start_seg . start . uuid and seg . start . uuid == start_seg . end . uuid ) or ( seg . start . uuid == start_seg . start . uuid and seg . end . uuid == start_seg . end . uuid ) ) : result . append ( self . points [ seg . start . uuid ] . position ( ) ) if index + 1 == len ( route_path . segments ) : if not ( ( seg . end . uuid == goal_seg . start . uuid and seg . start . uuid == goal_seg . end . uuid ) or ( seg . start . uuid == goal_seg . start . uuid and seg . end . uuid == goal_seg . end . uuid ) ) : result . append ( self . points [ seg . end . uuid ] . position ( ) ) else : result . append ( self . points [ seg . end . uuid ] . position ( ) ) p = self . _get_min_point ( goal_seg , goal_lot ) result . append ( p . toMsg ( ) ) dist_last_to_goal = self . distance2D ( p , goal_utm ) if dist_last_to_goal > self . _shift_to_route_within : result . append ( req . goal ) else : result . append ( req . start ) result . append ( req . goal ) last_utm = None dist = 0 for point in result : if last_utm is None : last_utm = geodesy . utm . fromMsg ( point ) else : new_utm = geodesy . utm . fromMsg ( point ) dist += self . distance2D ( last_utm , new_utm ) last_utm = new_utm return result , self . graph . id , start_seg . id , goal_seg . id , dist
Plan the shortest path between a start and a destination geopoint.
Plan the shortest path between a start and a destination geopoint .
""" Plan the shortest path between a start and a destination geopoint.

        The 'geo_path' method can receive GeoPoints out of the graph, upon such a case, the nearest segments on the OSM map are detected,
        and the planning is carried out.
        
        :pram req: The request message.
        :param req: geographic_msgs/GetGeoPath

        :return: The computed path, as well as the ids of the RouteNetwork and the start and end segments, plus the length of the path. The length is set to -1 in case of failure.
        :rtype: (geographic_msgs/GeoPoint[], uuid_msgs/UniqueID, uuid_msgs/UniqueID, uuid_msgs/UniqueID, length) 
        :raises: :exc:`ValueError` if invalid request.
        """ # check for possible errors in request # get the nearest segment to the start point # get the nearest segment to the destination point # determine the path # copy the route to the result list and adds the start and/or goal position to the path # add the start point if it is not close to the route # add only the endpoints of the segments # add the segment start point, if the plan segment and nearest segment are not equal # add the end point of the last segment, if the plan segment and nearest segment are not equal # add a perpendicular point or the nearest endpoint of the end segment # add the destination point if it is not close to the route # direct connection # calculate the distance
The computed path, as well as the ids of the RouteNetwork and the start and end segments, plus the length of the path. The length is set to -1 in case of failure.
The computed path as well as the ids of the RouteNetwork and the start and end segments plus the length of the path . The length is set to - 1 in case of failure .
(geographic_msgs/GeoPoint[], uuid_msgs/UniqueID, uuid_msgs/UniqueID, uuid_msgs/UniqueID, length)
:exc:`ValueError` if invalid request.
: exc : ` ValueError ` if invalid request .
self
req
None
pram
The request message.
The request message .