From bc8ab77a8358d893846715a06077c19bde96072c Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 24 Apr 2023 15:19:39 +0900 Subject: [PATCH] chore(lane_change): use BehaviorModuleOuput as output of generatePlannedPath (#3508) * chore(lane_change): use BehaviorModuleOuput as output of generatePlannedPath Signed-off-by: Takayuki Murooka * generatePlannedPath->generateOutput and generateExtendedDrivableArea->extendOutputDrivableArea Signed-off-by: Takayuki Murooka * move assignment in plan to generateOutput Signed-off-by: Muhammad Zulfaqar Azmi * fix pointer error in new module Signed-off-by: Muhammad Zulfaqar Azmi --------- Signed-off-by: Takayuki Murooka Signed-off-by: Muhammad Zulfaqar Azmi Co-authored-by: Muhammad Zulfaqar Azmi --- .../scene_module/lane_change/base_class.hpp | 4 ++-- .../external_request_lane_change_module.hpp | 2 +- .../scene_module/lane_change/normal.hpp | 4 ++-- .../external_request_lane_change_module.cpp | 10 ++++---- .../scene_module/lane_change/interface.cpp | 14 +++-------- .../lane_change/lane_change_module.cpp | 8 ++----- .../src/scene_module/lane_change/normal.cpp | 23 +++++++++++-------- 7 files changed, 30 insertions(+), 35 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index 6959fcb91616f..99dca7f3efa97 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -66,9 +66,9 @@ class LaneChangeBase virtual std::pair getSafePath(LaneChangePath & safe_path) const = 0; - virtual PathWithLaneId generatePlannedPath() = 0; + virtual BehaviorModuleOutput generateOutput() = 0; - virtual void generateExtendedDrivableArea(PathWithLaneId & path) = 0; + virtual void extendOutputDrivableArea(BehaviorModuleOutput & output) = 0; virtual bool hasFinishedLaneChange() const = 0; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request_lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request_lane_change_module.hpp index 157aeb9352bd8..0ae334abe5c93 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request_lane_change_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request_lane_change_module.hpp @@ -110,7 +110,7 @@ class ExternalRequestLaneChangeModule : public SceneModuleInterface LaneChangePath & safe_path) const; void updateLaneChangeStatus(); - void generateExtendedDrivableArea(PathWithLaneId & path); + void extendOutputDrivableArea(BehaviorModuleOutput & output); void updateOutputTurnSignal(BehaviorModuleOutput & output); void updateSteeringFactorPtr(const BehaviorModuleOutput & output); void updateSteeringFactorPtr( diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index 097db1ea6e065..eca898f9a59c1 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -43,9 +43,9 @@ class NormalLaneChange : public LaneChangeBase std::pair getSafePath(LaneChangePath & safe_path) const override; - PathWithLaneId generatePlannedPath() override; + BehaviorModuleOutput generateOutput() override; - void generateExtendedDrivableArea(PathWithLaneId & path) override; + void extendOutputDrivableArea(BehaviorModuleOutput & output) override; bool hasFinishedLaneChange() const override; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp index a6266e836ebf5..6238f9bb80b57 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp @@ -141,12 +141,13 @@ BehaviorModuleOutput ExternalRequestLaneChangeModule::plan() } } - generateExtendedDrivableArea(path); - prev_approved_path_ = path; BehaviorModuleOutput output; output.path = std::make_shared(path); + + extendOutputDrivableArea(output); + updateOutputTurnSignal(output); updateSteeringFactorPtr(output); @@ -619,7 +620,7 @@ std_msgs::msg::Header ExternalRequestLaneChangeModule::getRouteHeader() const { return planner_data_->route_handler->getRouteHeader(); } -void ExternalRequestLaneChangeModule::generateExtendedDrivableArea(PathWithLaneId & path) +void ExternalRequestLaneChangeModule::extendOutputDrivableArea(BehaviorModuleOutput & output) { const auto & common_parameters = planner_data_->parameters; const auto & route_handler = planner_data_->route_handler; @@ -629,8 +630,9 @@ void ExternalRequestLaneChangeModule::generateExtendedDrivableArea(PathWithLaneI const auto expanded_lanes = utils::expandLanelets( drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, dp.drivable_area_types_to_skip); + utils::generateDrivableArea( - path, expanded_lanes, common_parameters.vehicle_length, planner_data_); + *output.path, expanded_lanes, common_parameters.vehicle_length, planner_data_); } bool ExternalRequestLaneChangeModule::isApprovedPathSafe(Pose & ego_pose_before_collision) const diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 2d3f43866319a..726f88743be80 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -116,9 +116,6 @@ BehaviorModuleOutput LaneChangeInterface::plan() resetPathCandidate(); resetPathReference(); - module_type_->setPreviousDrivableLanes(getPreviousModuleOutput().drivable_lanes); - const auto path = module_type_->generatePlannedPath(); - if (!module_type_->isValidPath()) { return {}; } @@ -127,14 +124,9 @@ BehaviorModuleOutput LaneChangeInterface::plan() resetPathIfAbort(); } - const auto reference_path = module_type_->getReferencePath(); - - BehaviorModuleOutput output; - output.path = std::make_shared(path); - output.reference_path = std::make_shared(reference_path); - output.turn_signal_info = module_type_->updateOutputTurnSignal(); - - path_reference_ = std::make_shared(reference_path); + module_type_->setPreviousDrivableLanes(getPreviousModuleOutput().drivable_lanes); + auto output = module_type_->generateOutput(); + path_reference_ = output.reference_path; *prev_approved_path_ = *getPreviousModuleOutput().path; updateSteeringFactorPtr(output); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index 0e89367c48a95..73b7f6d823292 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -113,8 +113,6 @@ BehaviorModuleOutput LaneChangeModule::plan() resetPathReference(); is_activated_ = isActivated(); - const auto path = module_type_->generatePlannedPath(); - if (!module_type_->isValidPath()) { return {}; } @@ -123,12 +121,10 @@ BehaviorModuleOutput LaneChangeModule::plan() resetPathIfAbort(); } - BehaviorModuleOutput output; - output.path = std::make_shared(path); - output.turn_signal_info = module_type_->updateOutputTurnSignal(); + auto output = module_type_->generateOutput(); path_reference_ = getPreviousModuleOutput().reference_path; - prev_approved_path_ = path; + prev_approved_path_ = *output.path; updateSteeringFactorPtr(output); clearWaitingApproval(); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 61f4cb1e620b2..ebf95cef2b0b8 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -85,34 +85,39 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) return {true, found_safe_path}; } -PathWithLaneId NormalLaneChange::generatePlannedPath() +BehaviorModuleOutput NormalLaneChange::generateOutput() { - auto path = getLaneChangePath().path; - generateExtendedDrivableArea(path); + BehaviorModuleOutput output; + output.path = std::make_shared(getLaneChangePath().path); + + extendOutputDrivableArea(output); if (isAbortState()) { - return path; + return output; } if (isStopState()) { - const auto stop_point = utils::insertStopPoint(0.1, path); + const auto stop_point = utils::insertStopPoint(0.1, *output.path); } - return path; + output.reference_path = std::make_shared(getReferencePath()); + output.turn_signal_info = updateOutputTurnSignal(); + + return output; } -void NormalLaneChange::generateExtendedDrivableArea(PathWithLaneId & path) +void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) { const auto & common_parameters = planner_data_->parameters; const auto & dp = planner_data_->drivable_area_expansion_parameters; const auto drivable_lanes = getDrivableLanes(); - const auto shorten_lanes = utils::cutOverlappedLanes(path, drivable_lanes); + const auto shorten_lanes = utils::cutOverlappedLanes(*output.path, drivable_lanes); const auto expanded_lanes = utils::expandLanelets( shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, dp.drivable_area_types_to_skip); utils::generateDrivableArea( - path, expanded_lanes, common_parameters.vehicle_length, planner_data_); + *output.path, expanded_lanes, common_parameters.vehicle_length, planner_data_); } bool NormalLaneChange::hasFinishedLaneChange() const {