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.

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

feat(behavior_path_planner): revise behavior planner to work with dynamic lanelets #6393

Draft
wants to merge 15 commits into
base: main
Choose a base branch
from
3 changes: 3 additions & 0 deletions launch/tier4_planning_launch/launch/planning.launch.xml
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,8 @@
<arg name="planning_validator_param_path"/>
<!-- Auto mode setting-->
<arg name="enable_all_modules_auto_mode"/>
<!-- Differential lanelet map loading setting -->
<arg name="enable_differential_map_loading"/>
<arg name="is_simulation"/>

<group>
Expand All @@ -26,6 +28,7 @@
<push-ros-namespace namespace="scenario_planning"/>
<include file="$(find-pkg-share tier4_planning_launch)/launch/scenario_planning/scenario_planning.launch.xml">
<arg name="enable_all_modules_auto_mode" value="$(var enable_all_modules_auto_mode)"/>
<arg name="enable_differential_map_loading" value="$(var enable_differential_map_loading)"/>
<arg name="is_simulation" value="$(var is_simulation)"/>
</include>
</group>
Expand Down
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
<launch>
<arg name="enable_all_modules_auto_mode"/>
<arg name="enable_differential_map_loading"/>
<arg name="is_simulation"/>

<!-- lane_driving scenario -->
Expand All @@ -12,6 +13,7 @@
<include file="$(find-pkg-share tier4_planning_launch)/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml">
<arg name="container_type" value="component_container_mt"/>
<arg name="enable_all_modules_auto_mode" value="$(var enable_all_modules_auto_mode)"/>
<arg name="enable_differential_map_loading" value="$(var enable_differential_map_loading)"/>
<arg name="is_simulation" value="$(var is_simulation)"/>
<!-- This condition should be true if run_out module is enabled and its detection method is Points -->
<arg name="launch_compare_map_pipeline" value="false"/>
Expand Down
Original file line number Diff line number Diff line change
@@ -1,8 +1,12 @@
<launch>
<arg name="enable_all_modules_auto_mode"/>
<arg name="enable_differential_map_loading"/>

<arg name="input_route_topic_name" default="/planning/mission_planning/route"/>
<arg name="input_traffic_light_topic_name" default="/perception/traffic_light_recognition/traffic_signals"/>
<arg name="input_virtual_traffic_light_topic_name" default="/awapi/tmp/virtual_traffic_light_states"/>
<arg name="input_vector_map_topic_name" default="/map/vector_map"/>
<arg name="input_vector_map_topic_name" default="/map/vector_map" unless="$(var enable_differential_map_loading)"/>
<arg name="input_vector_map_topic_name" default="/map/dynamic_vector_map" if="$(var enable_differential_map_loading)"/>
<arg name="input_pointcloud_map_topic_name" default="/map/pointcloud_map"/>
<arg name="enable_all_modules_auto_mode"/>
<arg name="is_simulation"/>
Expand Down Expand Up @@ -192,6 +196,7 @@
<param from="$(var nearest_search_param_path)"/>
<param name="launch_modules" value="$(var behavior_path_planner_launch_modules)"/>
<param name="enable_all_modules_auto_mode" value="$(var enable_all_modules_auto_mode)"/>
<param name="enable_differential_map_loading" value="$(var enable_differential_map_loading)"/>
<param name="is_simulation" value="$(var is_simulation)"/>
<param from="$(var behavior_path_planner_side_shift_module_param_path)"/>
<param from="$(var behavior_path_planner_static_obstacle_avoidance_module_param_path)"/>
Expand Down
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
<launch>
<arg name="enable_all_modules_auto_mode"/>
<arg name="enable_differential_map_loading"/>
<arg name="is_simulation"/>
<!-- scenario selector -->
<group>
Expand Down Expand Up @@ -55,6 +56,7 @@
<group>
<include file="$(find-pkg-share tier4_planning_launch)/launch/scenario_planning/lane_driving.launch.xml">
<arg name="enable_all_modules_auto_mode" value="$(var enable_all_modules_auto_mode)"/>
<arg name="enable_differential_map_loading" value="$(var enable_differential_map_loading)"/>
<arg name="is_simulation" value="$(var is_simulation)"/>
</include>
</group>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -72,8 +72,9 @@ class RouteHandler
explicit RouteHandler(const LaneletMapBin & map_msg);

// non-const methods
void setMap(const LaneletMapBin & map_msg);
void setRoute(const LaneletRoute & route_msg);
void setMap(const LaneletMapBin & map_msg, const bool & is_enable_differential_lanelet = false);
void setRoute(
const LaneletRoute & route_msg, const bool & is_enable_differential_lanelet = false);
void setRouteLanelets(const lanelet::ConstLanelets & path_lanelets);
void clearRoute();

Expand Down Expand Up @@ -325,6 +326,7 @@ class RouteHandler
* @return vector of shoulder lanelets intersecting with given pose.
*/
lanelet::ConstLanelets getShoulderLaneletsAtPose(const Pose & pose) const;
lanelet::ConstLanelets getRouteLanelets() const;

private:
// MUST
Expand All @@ -348,14 +350,24 @@ class RouteHandler
Pose original_goal_pose_;

// non-const methods
void setLaneletsFromRouteMsg();
void setLaneletsFromRouteMsg(const bool & is_enable_differential_lanelet = false);

// const methods
// for routing
lanelet::ConstLanelets getMainLanelets(const lanelet::ConstLanelets & path_lanelets) const;

// for lanelet
lanelet::ConstLanelets getRouteLanelets() const;
bool isBijectiveConnection(
const lanelet::ConstLanelets & lanelet_section1,
const lanelet::ConstLanelets & lanelet_section2) const;
bool getPreviousLaneletWithinRouteExceptGoal(
const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * prev_lanelet) const;
bool getNextLaneletWithinRouteExceptStart(
const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * next_lanelet) const;
bool getRightLaneletWithinRoute(
const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * right_lanelet) const;
bool getLeftLaneletWithinRoute(
const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * left_lanelet) const;
lanelet::ConstLanelets getLaneletSequenceUpTo(
const lanelet::ConstLanelet & lanelet,
const double min_length = std::numeric_limits<double>::max(),
Expand Down
87 changes: 68 additions & 19 deletions planning/autoware_route_handler/src/route_handler.cpp
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2021-2024 TIER IV, Inc.

Check notice on line 1 in planning/autoware_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 1685 to 1734, 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/autoware_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.78 to 4.86, 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 @@ -182,7 +182,8 @@
route_ptr_ = nullptr;
}

void RouteHandler::setMap(const LaneletMapBin & map_msg)
void RouteHandler::setMap(
const LaneletMapBin & map_msg, const bool & is_enable_differential_lanelet)
{
lanelet_map_ptr_ = std::make_shared<lanelet::LaneletMap>();
lanelet::utils::conversion::fromBinMsg(
Expand Down Expand Up @@ -214,7 +215,7 @@
is_map_msg_ready_ = true;
is_handler_ready_ = false;

setLaneletsFromRouteMsg();
setLaneletsFromRouteMsg(is_enable_differential_lanelet);
}

bool RouteHandler::isRouteLooped(const RouteSections & route_sections)
Expand All @@ -231,7 +232,8 @@
return false;
}

void RouteHandler::setRoute(const LaneletRoute & route_msg)
void RouteHandler::setRoute(
const LaneletRoute & route_msg, const bool & is_enable_differential_lanelet)
{
if (!isRouteLooped(route_msg.segments)) {
// if get not modified route but new route, reset original start pose
Expand All @@ -241,7 +243,7 @@
}
route_ptr_ = std::make_shared<LaneletRoute>(route_msg);
is_handler_ready_ = false;
setLaneletsFromRouteMsg();
setLaneletsFromRouteMsg(is_enable_differential_lanelet);
} else {
RCLCPP_ERROR(
logger_,
Expand Down Expand Up @@ -364,48 +366,95 @@
is_handler_ready_ = false;
}

void RouteHandler::setLaneletsFromRouteMsg()
void RouteHandler::setLaneletsFromRouteMsg(const bool & is_enable_differential_lanelet)
{
if (!route_ptr_ || !is_map_msg_ready_) {
return;
}
route_lanelets_.clear();
preferred_lanelets_.clear();
const bool is_route_valid = lanelet::utils::route::isRouteValid(*route_ptr_, lanelet_map_ptr_);
if (!is_route_valid) {
return;
if (!is_enable_differential_lanelet) {
const bool is_route_valid = lanelet::utils::route::isRouteValid(*route_ptr_, lanelet_map_ptr_);
if (!is_route_valid) {
return;
}
}

size_t primitive_size{0};
for (const auto & route_section : route_ptr_->segments) {
primitive_size += route_section.primitives.size();
}
route_lanelets_.reserve(primitive_size);

for (const auto & route_section : route_ptr_->segments) {
for (const auto & primitive : route_section.primitives) {
const auto id = primitive.id;
const auto & llt = lanelet_map_ptr_->laneletLayer.get(id);
route_lanelets_.push_back(llt);
if (id == route_section.preferred_primitive.id) {
preferred_lanelets_.push_back(llt);
try {
const auto id = primitive.id;
const auto & llt = lanelet_map_ptr_->laneletLayer.get(id);
route_lanelets_.push_back(llt);
if (id == route_section.preferred_primitive.id) {
preferred_lanelets_.push_back(llt);
}
} catch (const std::exception & e) {
if (!is_enable_differential_lanelet) {
std::cerr
<< e.what()
<< ". Maybe the loaded route was created on a different Map from the current one. "
"Try to load the other Route again."
<< std::endl;
return;
} else {
RCLCPP_DEBUG(
logger_, "Failed to get lanelet differential map with id: %ld. %s", primitive.id,
e.what());
}
}
}
}
goal_lanelets_.clear();
start_lanelets_.clear();
if (!route_ptr_->segments.empty()) {
goal_lanelets_.reserve(route_ptr_->segments.back().primitives.size());
for (const auto & primitive : route_ptr_->segments.back().primitives) {
const auto id = primitive.id;
const auto & llt = lanelet_map_ptr_->laneletLayer.get(id);
goal_lanelets_.push_back(llt);
try {
const auto id = primitive.id;
const auto & llt = lanelet_map_ptr_->laneletLayer.get(id);
goal_lanelets_.push_back(llt);
} catch (const std::exception & e) {
if (!is_enable_differential_lanelet) {
std::cerr
<< e.what()
<< ". Maybe the loaded route was created on a different Map from the current one. "
"Try to load the other Route again."
<< std::endl;
return;
} else {
RCLCPP_DEBUG(
logger_, "Failed to get lanelet differential map with id: %ld. %s", primitive.id,
e.what());
}
}
}
start_lanelets_.reserve(route_ptr_->segments.front().primitives.size());
for (const auto & primitive : route_ptr_->segments.front().primitives) {
const auto id = primitive.id;
const auto & llt = lanelet_map_ptr_->laneletLayer.get(id);
start_lanelets_.push_back(llt);
try {
const auto id = primitive.id;
const auto & llt = lanelet_map_ptr_->laneletLayer.get(id);
start_lanelets_.push_back(llt);
} catch (const std::exception & e) {
if (!is_enable_differential_lanelet) {
std::cerr
<< e.what()
<< ". Maybe the loaded route was created on a different Map from the current one. "
"Try to load the other Route again."
<< std::endl;
return;
} else {
RCLCPP_DEBUG(
logger_, "Failed to get lanelet differential map with id: %ld. %s", primitive.id,
e.what());
}
}

Check warning on line 457 in planning/autoware_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::setLaneletsFromRouteMsg increases in cyclomatic complexity from 11 to 18, 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.

Check notice on line 457 in planning/autoware_route_handler/src/route_handler.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Bumpy Road Ahead

RouteHandler::setLaneletsFromRouteMsg increases from 2 to 4 logical blocks with deeply nested code, threshold is one single block per function. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.
}
}
is_handler_ready_ = true;
Expand Down
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/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.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 5.05 to 5.10, 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 @@ -219,6 +219,8 @@
p.ego_nearest_dist_threshold = declare_parameter<double>("ego_nearest_dist_threshold");
p.ego_nearest_yaw_threshold = declare_parameter<double>("ego_nearest_yaw_threshold");

p.enable_differential_map_loading = declare_parameter<bool>("enable_differential_map_loading");

return p;
}

Expand Down Expand Up @@ -402,15 +404,20 @@

// update map
if (map_ptr) {
planner_data_->route_handler->setMap(*map_ptr);
planner_data_->route_handler->setMap(
*map_ptr, planner_data_->parameters.enable_differential_map_loading);
if (planner_data_->parameters.enable_differential_map_loading) {
planner_manager_->resetCurrentRouteLanelet(planner_data_);
}
}

std::unique_lock<std::mutex> lk_manager(mutex_manager_); // for planner_manager_

// update route
const bool is_first_time = !(planner_data_->route_handler->isHandlerReady());
if (route_ptr) {
planner_data_->route_handler->setRoute(*route_ptr);
planner_data_->route_handler->setRoute(
*route_ptr, planner_data_->parameters.enable_differential_map_loading);

Check warning on line 420 in planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method

BehaviorPathPlannerNode::run increases in cyclomatic complexity from 18 to 19, 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.

Check notice on line 420 in planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Bumpy Road Ahead

BehaviorPathPlannerNode::run increases from 2 to 3 logical blocks with deeply nested code, threshold is one single block per function. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.
// uuid is not changed when rerouting with modified goal,
// in this case do not need to reset modules.
const bool has_same_route_id =
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -139,7 +139,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr<PlannerData> & da

const bool is_out_of_route = utils::isEgoOutOfRoute(
data->self_odometry->pose.pose, current_route_lanelet_->value(), data->prev_modified_goal,
data->route_handler);
data->route_handler, data->parameters);

if (!is_any_module_running && is_out_of_route) {
BehaviorModuleOutput result_output = utils::createGoalAroundPath(data);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -73,6 +73,9 @@ struct BehaviorPathPlannerParameters
double right_over_hang;
double base_link2front;
double base_link2rear;

// enable/disable differential map loading
bool enable_differential_map_loading;
};

#endif // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -237,7 +237,8 @@ bool isInLaneletWithYawThreshold(
bool isEgoOutOfRoute(
const Pose & self_pose, const lanelet::ConstLanelet & closest_road_lane,
const std::optional<PoseWithUuidStamped> & modified_goal,
const std::shared_ptr<RouteHandler> & route_handler);
const std::shared_ptr<RouteHandler> & route_handler,
const BehaviorPathPlannerParameters & common_param);

bool isEgoWithinOriginalLane(
const lanelet::ConstLanelets & current_lanes, const Pose & current_pose,
Expand Down
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2023 TIER IV, Inc.

Check notice on line 1 in planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.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 1296 to 1315, 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/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.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.38 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.

Check notice on line 1 in planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Primitive Obsession

The ratio of primitive types in function arguments decreases from 30.73% to 30.56%, threshold = 30.0%. The functions in this file have too many primitive types (e.g. int, double, float) in their function argument lists. Using many primitive types lead to the code smell Primitive Obsession. Avoid adding more primitive arguments.
//
// 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 @@ -493,17 +493,45 @@
bool isEgoOutOfRoute(
const Pose & self_pose, const lanelet::ConstLanelet & closest_road_lane,
const std::optional<PoseWithUuidStamped> & modified_goal,
const std::shared_ptr<RouteHandler> & route_handler)
const std::shared_ptr<RouteHandler> & route_handler,
const BehaviorPathPlannerParameters & common_param)
{
const Pose & goal_pose = (modified_goal && modified_goal->uuid == route_handler->getRouteUuid())
? modified_goal->pose
: route_handler->getGoalPose();

lanelet::ConstLanelet goal_lane;
const auto shoulder_goal_lanes = route_handler->getShoulderLaneletsAtPose(goal_pose);
if (!shoulder_goal_lanes.empty()) goal_lane = shoulder_goal_lanes.front();
const auto is_failed_getting_lanelet =
shoulder_goal_lanes.empty() && !route_handler->getGoalLanelet(&goal_lane);

bool is_failed_getting_lanelet;
if (!common_param.enable_differential_map_loading) {
is_failed_getting_lanelet =
shoulder_goal_lanes.empty() && !route_handler->getGoalLanelet(&goal_lane);
} else {
// If dynamic map is enabled, get the goal lanelet from the current lanelet map
//
// Check the route lanelets and last route element in the current lanelet map
// will be the goal lanelet

lanelet::LaneletMapPtr map = route_handler->getLaneletMapPtr();
lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(map);
lanelet::ConstLanelets route_lanelets = route_handler->getRouteLanelets();

auto goalLaneletIt =
std::find_if(all_lanelets.rbegin(), all_lanelets.rend(), [&](const auto & currentLanelet) {
return currentLanelet.id() == route_lanelets.back().id();
});

if (goalLaneletIt != all_lanelets.rend()) {
RCLCPP_DEBUG(
rclcpp::get_logger("behavior_path_planner").get_child("util"),
"Found goal lanelet in current lanelet map");
goal_lane = *goalLaneletIt;
is_failed_getting_lanelet = false;
}
}

Check notice on line 534 in planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ New issue: Excess Number of Function Arguments

isEgoOutOfRoute has 5 arguments, threshold = 4. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.

Check notice on line 534 in planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Bumpy Road Ahead

isEgoOutOfRoute increases from 2 to 3 logical blocks with deeply nested code, threshold is one single block per function. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.

Check warning on line 534 in planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method

isEgoOutOfRoute increases in cyclomatic complexity from 14 to 16, 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.
if (is_failed_getting_lanelet) {
RCLCPP_WARN_STREAM(
rclcpp::get_logger("behavior_path_planner").get_child("util"), "cannot find goal lanelet");
Expand Down
Loading