@@ -2133,24 +2133,17 @@ bool RouteHandler::planPathLaneletsBetweenCheckpoints(
2133
2133
lanelet::routing::LaneletPath shortest_path;
2134
2134
bool is_route_found = false ;
2135
2135
2136
- lanelet::routing::LaneletPath drivable_lane_path;
2137
- bool drivable_lane_path_found = false ;
2138
- double shortest_path_length2d = std::numeric_limits<double >::max ();
2136
+ double smallest_angle_diff = std::numeric_limits<double >::max ();
2137
+ constexpr double yaw_threshold = M_PI / 2.0 ;
2139
2138
2140
2139
for (const auto & st_llt : start_lanelets) {
2141
2140
// check if the angle difference between start_checkpoint and start lanelet center line
2142
2141
// orientation is in yaw_threshold range
2143
- double yaw_threshold = M_PI / 2.0 ;
2144
- bool is_proper_angle = false ;
2145
- {
2146
- double lanelet_angle = lanelet::utils::getLaneletAngle (st_llt, start_checkpoint.position );
2147
- double pose_yaw = tf2::getYaw (start_checkpoint.orientation );
2148
- double angle_diff = std::abs (autoware_utils::normalize_radian (lanelet_angle - pose_yaw));
2149
-
2150
- if (angle_diff <= std::abs (yaw_threshold)) {
2151
- is_proper_angle = true ;
2152
- }
2153
- }
2142
+ double lanelet_angle = lanelet::utils::getLaneletAngle (st_llt, start_checkpoint.position );
2143
+ double pose_yaw = tf2::getYaw (start_checkpoint.orientation );
2144
+ double angle_diff = std::abs (autoware_utils::normalize_radian (lanelet_angle - pose_yaw));
2145
+
2146
+ bool is_proper_angle = angle_diff <= std::abs (yaw_threshold);
2154
2147
2155
2148
optional_route = routing_graph_ptr_->getRoute (st_llt, goal_lanelet, 0 );
2156
2149
if (!optional_route || !is_proper_angle) {
@@ -2161,34 +2154,28 @@ bool RouteHandler::planPathLaneletsBetweenCheckpoints(
2161
2154
<< " - goal checkpoint: " << toString (goal_checkpoint) << std::endl
2162
2155
<< " - start lane id: " << st_llt.id () << std::endl
2163
2156
<< " - goal lane id: " << goal_lanelet.id () << std::endl);
2164
- } else {
2165
- is_route_found = true ;
2166
-
2167
- if (optional_route->length2d () < shortest_path_length2d) {
2168
- shortest_path_length2d = optional_route->length2d ();
2169
- shortest_path = optional_route->shortestPath ();
2170
- start_lanelet = st_llt;
2171
- }
2157
+ continue ;
2158
+ }
2159
+ is_route_found = true ;
2160
+ if (angle_diff < smallest_angle_diff) {
2161
+ smallest_angle_diff = angle_diff;
2162
+ shortest_path = optional_route->shortestPath ();
2163
+ start_lanelet = st_llt;
2172
2164
}
2173
2165
}
2174
2166
2175
2167
if (is_route_found) {
2176
2168
lanelet::routing::LaneletPath path;
2177
- if (consider_no_drivable_lanes) {
2178
- bool shortest_path_has_no_drivable_lane = hasNoDrivableLaneInPath (shortest_path);
2179
- if (shortest_path_has_no_drivable_lane) {
2169
+ path = [&]() -> lanelet::routing::LaneletPath {
2170
+ if (!consider_no_drivable_lanes) return shortest_path;
2171
+ lanelet::routing::LaneletPath drivable_lane_path;
2172
+ bool drivable_lane_path_found = false ;
2173
+ if (hasNoDrivableLaneInPath (shortest_path)) {
2180
2174
drivable_lane_path_found =
2181
2175
findDrivableLanePath (start_lanelet, goal_lanelet, drivable_lane_path);
2182
2176
}
2183
-
2184
- if (drivable_lane_path_found) {
2185
- path = drivable_lane_path;
2186
- } else {
2187
- path = shortest_path;
2188
- }
2189
- } else {
2190
- path = shortest_path;
2191
- }
2177
+ return (drivable_lane_path_found) ? drivable_lane_path : shortest_path;
2178
+ }();
2192
2179
2193
2180
path_lanelets->reserve (path.size ());
2194
2181
for (const auto & llt : path) {
0 commit comments