Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

feat(routing_no_drivable_lane_when_module_enabled): add solution for routing no_drivable_lane only when module enabled #4308

Merged
merged 5 commits into from
Aug 7, 2023
Merged

feat(routing_no_drivable_lane_when_module_enabled): add solution for routing no_drivable_lane only when module enabled #4308

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
5443dd0
feat(routing_no_drivable_lane_when_module_enabled): add proposed solu…
ahmeddesokyebrahim Jul 18, 2023
f24791b
feat(routing_no_drivable_lane_when_module_enabled): improving comment…
ahmeddesokyebrahim Jul 18, 2023
5ea8781
style(pre-commit): autofix
pre-commit-ci[bot] Jul 18, 2023
6dc2f1d
style(pre-commit): autofix
pre-commit-ci[bot] Jul 20, 2023
0eac8f6
feat(routing_no_drivable_lane_when_module_enabled): reverting file ch…
ahmeddesokyebrahim Jul 20, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
21 changes: 11 additions & 10 deletions planning/mission_planner/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,16 +14,17 @@ In current Autoware.universe, only Lanelet2 map format is supported.

### Parameters

| Name | Type | Description |
| -------------------------- | ------ | ---------------------------------------------------------------------------------------------------------------- |
| `map_frame` | string | The frame name for map |
| `arrival_check_angle_deg` | double | Angle threshold for goal check |
| `arrival_check_distance` | double | Distance threshold for goal check |
| `arrival_check_duration` | double | Duration threshold for goal check |
| `goal_angle_threshold` | double | Max goal pose angle for goal approve |
| `enable_correct_goal_pose` | bool | Enabling correction of goal pose according to the closest lanelet orientation |
| `reroute_time_threshold` | double | If the time to the rerouting point at the current velocity is greater than this threshold, rerouting is possible |
| `minimum_reroute_length` | double | Minimum Length for publishing a new route |
| Name | Type | Description |
| ---------------------------- | ------ | ---------------------------------------------------------------------------------------------------------------- |
| `map_frame` | string | The frame name for map |
| `arrival_check_angle_deg` | double | Angle threshold for goal check |
| `arrival_check_distance` | double | Distance threshold for goal check |
| `arrival_check_duration` | double | Duration threshold for goal check |
| `goal_angle_threshold` | double | Max goal pose angle for goal approve |
| `enable_correct_goal_pose` | bool | Enabling correction of goal pose according to the closest lanelet orientation |
| `reroute_time_threshold` | double | If the time to the rerouting point at the current velocity is greater than this threshold, rerouting is possible |
| `minimum_reroute_length` | double | Minimum Length for publishing a new route |
| `consider_no_drivable_lanes` | bool | This flag is for considering no_drivable_lanes in planning or not. |

### Services

Expand Down
1 change: 1 addition & 0 deletions planning/mission_planner/config/mission_planner.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,3 +8,4 @@
enable_correct_goal_pose: false
reroute_time_threshold: 10.0
minimum_reroute_length: 30.0
consider_no_drivable_lanes: false # This flag is for considering no_drivable_lanes in planning or not.
3 changes: 2 additions & 1 deletion planning/mission_planner/src/lanelet2_plugins/default_planner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -147,6 +147,7 @@ void DefaultPlanner::initialize_common(rclcpp::Node * node)
vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo();
param_.goal_angle_threshold_deg = node_->declare_parameter<double>("goal_angle_threshold_deg");
param_.enable_correct_goal_pose = node_->declare_parameter<bool>("enable_correct_goal_pose");
param_.consider_no_drivable_lanes = node_->declare_parameter<bool>("consider_no_drivable_lanes");
}

void DefaultPlanner::initialize(rclcpp::Node * node)
Expand Down Expand Up @@ -401,7 +402,7 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points)
const auto goal_check_point = points.at(i);
lanelet::ConstLanelets path_lanelets;
if (!route_handler_.planPathLaneletsBetweenCheckpoints(
start_check_point, goal_check_point, &path_lanelets)) {
start_check_point, goal_check_point, &path_lanelets, param_.consider_no_drivable_lanes)) {
RCLCPP_WARN(logger, "Failed to plan route.");
return route_msg;
}
Expand Down
1 change: 1 addition & 0 deletions planning/mission_planner/src/lanelet2_plugins/default_planner.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@ struct DefaultPlannerParameters
{
double goal_angle_threshold_deg;
bool enable_correct_goal_pose;
bool consider_no_drivable_lanes;
};

class DefaultPlanner : public mission_planner::PlannerPlugin
Expand Down
2 changes: 1 addition & 1 deletion planning/route_handler/include/route_handler/route_handler.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -89,7 +89,7 @@ class RouteHandler
// for routing
bool planPathLaneletsBetweenCheckpoints(
const Pose & start_checkpoint, const Pose & goal_checkpoint,
lanelet::ConstLanelets * path_lanelets) const;
lanelet::ConstLanelets * path_lanelets, const bool consider_no_drivable_lanes = false) const;
std::vector<LaneletSegment> createMapSegments(const lanelet::ConstLanelets & path_lanelets) const;
static bool isRouteLooped(const RouteSections & route_sections);

Expand Down
22 changes: 13 additions & 9 deletions planning/route_handler/src/route_handler.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2021-2023 Tier IV, Inc.

Check notice on line 1 in planning/route_handler/src/route_handler.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Lines of Code in a Single File 

The lines of code increases from 1800 to 1804, improve code health by reducing it to 1000. The number of Lines of Code in a single file. More Lines of Code lowers the code health.

Check notice on line 1 in planning/route_handler/src/route_handler.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Overall Code Complexity 

The mean cyclomatic complexity increases from 4.40 to 4.41, threshold = 4. This file has many conditional statements (e.g. if, for, while) across its implementation, leading to lower code health. Avoid adding more conditionals.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -1974,79 +1974,83 @@

bool RouteHandler::planPathLaneletsBetweenCheckpoints(
const Pose & start_checkpoint, const Pose & goal_checkpoint,
lanelet::ConstLanelets * path_lanelets) const
lanelet::ConstLanelets * path_lanelets, const bool consider_no_drivable_lanes) const
{
// Find lanelets for start point. First, find all lanelets containing the start point to calculate
// all possible route later. It fails when the point is not located on any road_lanelet (e.g. the
// start point is located out of any lanelets or road_shoulder lanelet which is not contained in
// road_lanelet). In that case, find the closest lanelet instead.
lanelet::ConstLanelet start_lanelet;
lanelet::ConstLanelets start_lanelets;
if (!lanelet::utils::query::getCurrentLanelets(
road_lanelets_, start_checkpoint, &start_lanelets)) {
if (!lanelet::utils::query::getClosestLanelet(
road_lanelets_, start_checkpoint, &start_lanelet)) {
RCLCPP_WARN_STREAM(
logger_, "Failed to find current lanelet."
<< std::endl
<< " - start checkpoint: " << toString(start_checkpoint) << std::endl
<< " - goal checkpoint: " << toString(goal_checkpoint) << std::endl);
return false;
}
start_lanelets = {start_lanelet};
}

// Find lanelets for goal point.
lanelet::ConstLanelet goal_lanelet;
if (!lanelet::utils::query::getClosestLanelet(road_lanelets_, goal_checkpoint, &goal_lanelet)) {
RCLCPP_WARN_STREAM(
logger_, "Failed to find closest lanelet."
<< std::endl
<< " - start checkpoint: " << toString(start_checkpoint) << std::endl
<< " - goal checkpoint: " << toString(goal_checkpoint) << std::endl);
return false;
}

lanelet::Optional<lanelet::routing::Route> optional_route;
std::vector<lanelet::ConstLanelets> candidate_paths;
lanelet::routing::LaneletPath shortest_path;
bool is_route_found = false;

lanelet::routing::LaneletPath drivable_lane_path;
bool drivable_lane_path_found = false;
double shortest_path_length2d = std::numeric_limits<double>::max();

for (const auto & st_llt : start_lanelets) {
optional_route = routing_graph_ptr_->getRoute(st_llt, goal_lanelet, 0);
if (!optional_route) {
RCLCPP_ERROR_STREAM(
logger_, "Failed to find a proper path!"
<< std::endl
<< " - start checkpoint: " << toString(start_checkpoint) << std::endl
<< " - goal checkpoint: " << toString(goal_checkpoint) << std::endl
<< " - start lane id: " << st_llt.id() << std::endl
<< " - goal lane id: " << goal_lanelet.id() << std::endl);
} else {
is_route_found = true;

if (optional_route->length2d() < shortest_path_length2d) {
shortest_path_length2d = optional_route->length2d();
shortest_path = optional_route->shortestPath();
start_lanelet = st_llt;
}
}
}

if (is_route_found) {
bool shortest_path_has_no_drivable_lane = hasNoDrivableLaneInPath(shortest_path);
if (shortest_path_has_no_drivable_lane) {
drivable_lane_path_found =
findDrivableLanePath(start_lanelet, goal_lanelet, drivable_lane_path);
}

lanelet::routing::LaneletPath path;
if (drivable_lane_path_found) {
path = drivable_lane_path;
if (consider_no_drivable_lanes) {
bool shortest_path_has_no_drivable_lane = hasNoDrivableLaneInPath(shortest_path);
if (shortest_path_has_no_drivable_lane) {
drivable_lane_path_found =
findDrivableLanePath(start_lanelet, goal_lanelet, drivable_lane_path);
}

if (drivable_lane_path_found) {
path = drivable_lane_path;
} else {
path = shortest_path;
}

Check warning on line 2053 in planning/route_handler/src/route_handler.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method 

RouteHandler::planPathLaneletsBetweenCheckpoints increases in cyclomatic complexity from 11 to 12, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
} else {
path = shortest_path;
}
Expand Down
Loading