@@ -41,9 +41,7 @@ std::vector<LaneKey> RoutingGraph::get_lane_predecessors(const LaneKey& lane_key
4141RoutingPath RoutingGraph::shortest_path (const LaneKey& from, const LaneKey& to) const
4242{
4343 if (from == to)
44- {
4544 return {from};
46- }
4745
4846 // Priority queue to maintain open set
4947 std::priority_queue<WeightedLaneKey, std::vector<WeightedLaneKey>, std::greater<odr::WeightedLaneKey>> open_set;
@@ -65,9 +63,7 @@ RoutingPath RoutingGraph::shortest_path(const LaneKey& from, const LaneKey& to)
6563 // Skip stale entries
6664 auto current_cost_itr = cost_from_start.find (current);
6765 if (current_cost_itr == cost_from_start.end () || current_weighted.weight > current_cost_itr->second )
68- {
6966 continue ;
70- }
7167
7268 // If the goal is reached, reconstruct the path
7369 if (current == to)
@@ -79,9 +75,7 @@ RoutingPath RoutingGraph::shortest_path(const LaneKey& from, const LaneKey& to)
7975 path.push_back (at);
8076 auto it = came_from.find (at);
8177 if (it == came_from.end ())
82- {
8378 break ;
84- }
8579 at = it->second ;
8680 }
8781 std::reverse (path.begin (), path.end ());
@@ -91,9 +85,7 @@ RoutingPath RoutingGraph::shortest_path(const LaneKey& from, const LaneKey& to)
9185 // Get successors of the current node
9286 auto succ_itr = lane_key_to_successors.find (current);
9387 if (succ_itr == lane_key_to_successors.end ())
94- {
9588 continue ;
96- }
9789
9890 for (const auto & weighted_successor : succ_itr->second )
9991 {
@@ -110,24 +102,16 @@ RoutingPath RoutingGraph::shortest_path(const LaneKey& from, const LaneKey& to)
110102 {
111103 // Update cost_from_start
112104 if (neighbor_cost_itr == cost_from_start.end ())
113- {
114105 cost_from_start.emplace (neighbor, alternative_path_cost);
115- }
116106 else
117- {
118107 neighbor_cost_itr->second = alternative_path_cost;
119- }
120108
121109 // Update came_from
122110 auto came_from_itr = came_from.find (neighbor);
123111 if (came_from_itr == came_from.end ())
124- {
125112 came_from.emplace (neighbor, current);
126- }
127113 else
128- {
129114 came_from_itr->second = current;
130- }
131115
132116 // Add the neighbor to the open set
133117 open_set.emplace (neighbor, alternative_path_cost);
0 commit comments