chore(lane_change): use BehaviorModuleOuput as output of generatePlan…

…nedPath (autowarefoundation#3508) * chore(lane_change): use BehaviorModuleOuput as output of generatePlannedPath Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * generatePlannedPath->generateOutput and generateExtendedDrivableArea->extendOutputDrivableArea Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * move assignment in plan to generateOutput Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * fix pointer error in new module Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> Co-authored-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
kenji-miyake · Apr 24, 2023 · bc8ab77 · bc8ab77
1 parent 2a0f760
commit bc8ab77
Show file tree

Hide file tree

Showing 7 changed files with 30 additions and 35 deletions.
diff --git a/...havior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/...havior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp
@@ -66,9 +66,9 @@ class LaneChangeBase
 
   virtual std::pair<bool, bool> 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/...de/behavior_path_planner/scene_module/lane_change/external_request_lane_change_module.hpp b/...de/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/...g/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/...g/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp
@@ -43,9 +43,9 @@ class NormalLaneChange : public LaneChangeBase
 
   std::pair<bool, bool> 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/...ehavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp b/...ehavior_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<PathWithLaneId>(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
@@ -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<PathWithLaneId>(path);
-  output.reference_path = std::make_shared<PathWithLaneId>(reference_path);
-  output.turn_signal_info = module_type_->updateOutputTurnSignal();
-
-  path_reference_ = std::make_shared<PathWithLaneId>(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
@@ -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<PathWithLaneId>(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
@@ -85,34 +85,39 @@ std::pair<bool, bool> 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<PathWithLaneId>(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<PathWithLaneId>(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
 {