Skip to content

Commit

Permalink
generatePlannedPath->generateOutput and generateExtendedDrivableArea-…
Browse files Browse the repository at this point in the history
…>extendOutputDrivableArea

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
  • Loading branch information
takayuki5168 committed Apr 24, 2023
1 parent 2432ee3 commit 0e0c6bc
Show file tree
Hide file tree
Showing 7 changed files with 13 additions and 13 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -66,9 +66,9 @@ class LaneChangeBase

virtual std::pair<bool, bool> getSafePath(LaneChangePath & safe_path) const = 0;

virtual BehaviorModuleOutput generatePlannedPath() = 0;
virtual BehaviorModuleOutput generateOutput() = 0;

virtual void generateExtendedDrivableArea(BehaviorModuleOutput & output) = 0;
virtual void extendOutputDrivableArea(BehaviorModuleOutput & output) = 0;

virtual bool hasFinishedLaneChange() const = 0;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -110,7 +110,7 @@ class ExternalRequestLaneChangeModule : public SceneModuleInterface
LaneChangePath & safe_path) const;

void updateLaneChangeStatus();
void generateExtendedDrivableArea(BehaviorModuleOutput & output);
void extendOutputDrivableArea(BehaviorModuleOutput & output);
void updateOutputTurnSignal(BehaviorModuleOutput & output);
void updateSteeringFactorPtr(const BehaviorModuleOutput & output);
void updateSteeringFactorPtr(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -43,9 +43,9 @@ class NormalLaneChange : public LaneChangeBase

std::pair<bool, bool> getSafePath(LaneChangePath & safe_path) const override;

BehaviorModuleOutput generatePlannedPath() override;
BehaviorModuleOutput generateOutput() override;

void generateExtendedDrivableArea(BehaviorModuleOutput & output) override;
void extendOutputDrivableArea(BehaviorModuleOutput & output) override;

bool hasFinishedLaneChange() const override;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -146,7 +146,7 @@ BehaviorModuleOutput ExternalRequestLaneChangeModule::plan()
BehaviorModuleOutput output;
output.path = std::make_shared<PathWithLaneId>(path);

generateExtendedDrivableArea(output);
extendOutputDrivableArea(output);

updateOutputTurnSignal(output);

Expand Down Expand Up @@ -620,7 +620,7 @@ std_msgs::msg::Header ExternalRequestLaneChangeModule::getRouteHeader() const
{
return planner_data_->route_handler->getRouteHeader();
}
void ExternalRequestLaneChangeModule::generateExtendedDrivableArea(BehaviorModuleOutput & output)
void ExternalRequestLaneChangeModule::extendOutputDrivableArea(BehaviorModuleOutput & output)
{
const auto & common_parameters = planner_data_->parameters;
const auto & route_handler = planner_data_->route_handler;
Expand All @@ -632,7 +632,7 @@ void ExternalRequestLaneChangeModule::generateExtendedDrivableArea(BehaviorModul
dp.drivable_area_types_to_skip);

utils::generateDrivableArea(
output.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
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -117,7 +117,7 @@ BehaviorModuleOutput LaneChangeInterface::plan()
resetPathReference();

module_type_->setPreviousDrivableLanes(getPreviousModuleOutput().drivable_lanes);
auto output = module_type_->generatePlannedPath();
auto output = module_type_->generateOutput();

if (!module_type_->isValidPath()) {
return {};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -113,7 +113,7 @@ BehaviorModuleOutput LaneChangeModule::plan()
resetPathReference();
is_activated_ = isActivated();

auto output = module_type_->generatePlannedPath();
auto output = module_type_->generateOutput();

if (!module_type_->isValidPath()) {
return {};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -85,12 +85,12 @@ std::pair<bool, bool> NormalLaneChange::getSafePath(LaneChangePath & safe_path)
return {true, found_safe_path};
}

BehaviorModuleOutput NormalLaneChange::generatePlannedPath()
BehaviorModuleOutput NormalLaneChange::generateOutput()
{
BehaviorModuleOutput output;
output.path = std::make_shared<PathWithLaneId>(getLaneChangePath().path);

generateExtendedDrivableArea(output);
extendOutputDrivableArea(output);

if (isAbortState()) {
return output;
Expand All @@ -103,7 +103,7 @@ BehaviorModuleOutput NormalLaneChange::generatePlannedPath()
return output;
}

void NormalLaneChange::generateExtendedDrivableArea(BehaviorModuleOutput & output)
void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output)
{
const auto & common_parameters = planner_data_->parameters;
const auto & dp = planner_data_->drivable_area_expansion_parameters;
Expand Down

0 comments on commit 0e0c6bc

Please sign in to comment.