A starting point, a set of one or more way points, and a destination point of a route along which the ADV is to be driven are determined. All lane segments near the starting point, the set of way points, and the destination point within a predetermined threshold distance are determined respectively. A set of route candidates are determined with an A-star (A*) searching algorithm based on a set of nodes representing all the lane segments near the starting point, the set of way points, and the destination point respectively. The route is selected from the set of route candidates based on respective costs of the set of route candidates. The ADV is being controlled to drive along the selected route autonomously.