Skip to content

Commit 81d5374

Browse files
feat(route_handler): use start pose yaw and not lanelet length to select start lanelet (#6550)
use start pose yaw and not lanelet length to select start lanelet Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
1 parent 0e1761a commit 81d5374

File tree

1 file changed

+21
-34
lines changed

1 file changed

+21
-34
lines changed

planning/route_handler/src/route_handler.cpp

+21-34
Original file line numberDiff line numberDiff line change
@@ -2133,24 +2133,17 @@ bool RouteHandler::planPathLaneletsBetweenCheckpoints(
21332133
lanelet::routing::LaneletPath shortest_path;
21342134
bool is_route_found = false;
21352135

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;
21392138

21402139
for (const auto & st_llt : start_lanelets) {
21412140
// check if the angle difference between start_checkpoint and start lanelet center line
21422141
// 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);
21542147

21552148
optional_route = routing_graph_ptr_->getRoute(st_llt, goal_lanelet, 0);
21562149
if (!optional_route || !is_proper_angle) {
@@ -2161,34 +2154,28 @@ bool RouteHandler::planPathLaneletsBetweenCheckpoints(
21612154
<< " - goal checkpoint: " << toString(goal_checkpoint) << std::endl
21622155
<< " - start lane id: " << st_llt.id() << std::endl
21632156
<< " - 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;
21722164
}
21732165
}
21742166

21752167
if (is_route_found) {
21762168
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)) {
21802174
drivable_lane_path_found =
21812175
findDrivableLanePath(start_lanelet, goal_lanelet, drivable_lane_path);
21822176
}
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+
}();
21922179

21932180
path_lanelets->reserve(path.size());
21942181
for (const auto & llt : path) {

0 commit comments

Comments
 (0)