fix(behavior_path_planner): use lane following params in new framework

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -35,6 +35,7 @@
 #include "behavior_path_planner/turn_signal_decider.hpp"
 #include "behavior_path_planner/util/avoidance/avoidance_module_data.hpp"
 #include "behavior_path_planner/util/lane_change/lane_change_module_data.hpp"
+#include "behavior_path_planner/util/lane_following/module_data.hpp"
 #include "behavior_path_planner/util/pull_out/pull_out_parameters.hpp"
 #include "behavior_path_planner/util/pull_over/pull_over_parameters.hpp"
 #include "behavior_path_planner/util/side_shift/side_shift_parameters.hpp"
@@ -153,16 +154,17 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   // parameters
   std::shared_ptr<AvoidanceParameters> avoidance_param_ptr_;
   std::shared_ptr<LaneChangeParameters> lane_change_param_ptr_;
+  std::shared_ptr<LaneFollowingParameters> lane_following_param_ptr_;
 
   BehaviorPathPlannerParameters getCommonParam();
 
 #ifdef USE_OLD_ARCHITECTURE
   BehaviorTreeManagerParam getBehaviorTreeManagerParam();
-  LaneFollowingParameters getLaneFollowingParam();
 #endif
 
   AvoidanceParameters getAvoidanceParam();
   LaneChangeParameters getLaneChangeParam();
+  LaneFollowingParameters getLaneFollowingParam();
   SideShiftParameters getSideShiftParam();
   PullOverParameters getPullOverParam();
   PullOutParameters getPullOutParam();

planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp

@@ -17,6 +17,7 @@
 
 #include "behavior_path_planner/scene_module/scene_module_interface.hpp"
 #include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp"
+#include "behavior_path_planner/util/lane_following/module_data.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -54,7 +55,9 @@ struct SceneModuleStatus
 class PlannerManager
 {
 public:
-  PlannerManager(rclcpp::Node & node, const bool verbose);
+  PlannerManager(
+    rclcpp::Node & node, const std::shared_ptr<LaneFollowingParameters> & parameters,
+    const bool verbose);
 
   BehaviorModuleOutput run(const std::shared_ptr<PlannerData> & data);
 
@@ -187,6 +190,8 @@ class PlannerManager
 
   boost::optional<lanelet::ConstLanelet> root_lanelet_{boost::none};
 
+  std::shared_ptr<LaneFollowingParameters> parameters_;
+
   std::vector<SceneModuleManagerPtr> manager_ptrs_;
 
   std::vector<SceneModulePtr> approved_module_ptrs_;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_following/lane_following_module.hpp

@@ -16,6 +16,7 @@
 #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_FOLLOWING__LANE_FOLLOWING_MODULE_HPP_
 
 #include "behavior_path_planner/scene_module/scene_module_interface.hpp"
+#include "behavior_path_planner/util/lane_following/module_data.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -29,20 +30,12 @@ namespace behavior_path_planner
 {
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 
-struct LaneFollowingParameters
-{
-  double lane_change_prepare_duration;
-  // drivable area expansion
-  double drivable_area_right_bound_offset;
-  double drivable_area_left_bound_offset;
-  std::vector<std::string> drivable_area_types_to_skip{};
-};
-
 class LaneFollowingModule : public SceneModuleInterface
 {
 public:
   LaneFollowingModule(
-    const std::string & name, rclcpp::Node & node, const LaneFollowingParameters & parameters);
+    const std::string & name, rclcpp::Node & node,
+    const std::shared_ptr<LaneFollowingParameters> & parameters);
 
   bool isExecutionRequested() const override;
   bool isExecutionReady() const override;
@@ -52,14 +45,14 @@ class LaneFollowingModule : public SceneModuleInterface
   void onEntry() override;
   void onExit() override;
 
-  void setParameters(const LaneFollowingParameters & parameters);
+  void setParameters(const std::shared_ptr<LaneFollowingParameters> & parameters);
   void acceptVisitor(
     [[maybe_unused]] const std::shared_ptr<SceneModuleVisitor> & visitor) const override
   {
   }
 
 private:
-  LaneFollowingParameters parameters_;
+  std::shared_ptr<LaneFollowingParameters> parameters_;
 
   PathWithLaneId getReferencePath() const;
   void initParam();

planning/behavior_path_planner/include/behavior_path_planner/util/lane_following/module_data.hpp

@@ -0,0 +1,35 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BEHAVIOR_PATH_PLANNER__UTIL__LANE_FOLLOWING__MODULE_DATA_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTIL__LANE_FOLLOWING__MODULE_DATA_HPP_
+
+#include <string>
+#include <vector>
+
+namespace behavior_path_planner
+{
+
+struct LaneFollowingParameters
+{
+  double lane_change_prepare_duration;
+  // drivable area expansion
+  double drivable_area_right_bound_offset;
+  double drivable_area_left_bound_offset;
+  std::vector<std::string> drivable_area_types_to_skip{};
+};
+
+}  // namespace behavior_path_planner
+
+#endif  // BEHAVIOR_PATH_PLANNER__UTIL__LANE_FOLLOWING__MODULE_DATA_HPP_

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -118,6 +118,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
   {
     avoidance_param_ptr_ = std::make_shared<AvoidanceParameters>(getAvoidanceParam());
     lane_change_param_ptr_ = std::make_shared<LaneChangeParameters>(getLaneChangeParam());
+    lane_following_param_ptr_ = std::make_shared<LaneFollowingParameters>(getLaneFollowingParam());
   }
 
   m_set_param_res = this->add_on_set_parameters_callback(
@@ -144,7 +145,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
     bt_manager_->registerSceneModule(avoidance_module);
 
     auto lane_following_module =
-      std::make_shared<LaneFollowingModule>("LaneFollowing", *this, getLaneFollowingParam());
+      std::make_shared<LaneFollowingModule>("LaneFollowing", *this, lane_following_param_ptr_);
     bt_manager_->registerSceneModule(lane_following_module);
 
     auto ext_request_lane_change_right_module =
@@ -193,7 +194,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
     mutex_bt_.lock();
 
     const auto & p = planner_data_->parameters;
-    planner_manager_ = std::make_shared<PlannerManager>(*this, p.verbose);
+    planner_manager_ =
+      std::make_shared<PlannerManager>(*this, lane_following_param_ptr_, p.verbose);
 
     mutex_bt_.unlock();
   }
@@ -487,7 +489,6 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
   return p;
 }
 
-#ifdef USE_OLD_ARCHITECTURE
 LaneFollowingParameters BehaviorPathPlannerNode::getLaneFollowingParam()
 {
   LaneFollowingParameters p{};
@@ -501,7 +502,6 @@ LaneFollowingParameters BehaviorPathPlannerNode::getLaneFollowingParam()
     declare_parameter("lane_following.lane_change_prepare_duration", 2.0);
   return p;
 }
-#endif
 
 LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam()
 {

planning/behavior_path_planner/src/planner_manager.cpp

@@ -27,8 +27,11 @@
 namespace behavior_path_planner
 {
 
-PlannerManager::PlannerManager(rclcpp::Node & node, const bool verbose)
-: logger_(node.get_logger().get_child("planner_manager")),
+PlannerManager::PlannerManager(
+  rclcpp::Node & node, const std::shared_ptr<LaneFollowingParameters> & parameters,
+  const bool verbose)
+: parameters_{parameters},
+  logger_(node.get_logger().get_child("planner_manager")),
   clock_(*node.get_clock()),
   verbose_{verbose}
 {
@@ -355,12 +358,15 @@ BehaviorModuleOutput PlannerManager::getReferencePath(
     const double lane_change_buffer = util::calcLaneChangeBuffer(p, num_lane_change);
 
     reference_path = util::setDecelerationVelocity(
-      *route_handler, reference_path, current_lanes, 2.0, lane_change_buffer);
+      *route_handler, reference_path, current_lanes, parameters_->lane_change_prepare_duration,
+      lane_change_buffer);
   }
 
   const auto shorten_lanes = util::cutOverlappedLanes(reference_path, drivable_lanes);
 
-  const auto expanded_lanes = util::expandLanelets(shorten_lanes, 0.0, 0.0);
+  const auto expanded_lanes = util::expandLanelets(
+    shorten_lanes, parameters_->drivable_area_left_bound_offset,
+    parameters_->drivable_area_right_bound_offset, parameters_->drivable_area_types_to_skip);
 
   util::generateDrivableArea(reference_path, expanded_lanes, p.vehicle_length, data);
 

planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp

@@ -25,7 +25,8 @@
 namespace behavior_path_planner
 {
 LaneFollowingModule::LaneFollowingModule(
-  const std::string & name, rclcpp::Node & node, const LaneFollowingParameters & parameters)
+  const std::string & name, rclcpp::Node & node,
+  const std::shared_ptr<LaneFollowingParameters> & parameters)
 : SceneModuleInterface{name, node}, parameters_{parameters}
 {
   initParam();
@@ -69,7 +70,7 @@ void LaneFollowingModule::onExit()
   RCLCPP_DEBUG(getLogger(), "LANE_FOLLOWING onExit");
 }
 
-void LaneFollowingModule::setParameters(const LaneFollowingParameters & parameters)
+void LaneFollowingModule::setParameters(const std::shared_ptr<LaneFollowingParameters> & parameters)
 {
   parameters_ = parameters;
 }
@@ -146,15 +147,15 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const
     const double lane_change_buffer = util::calcLaneChangeBuffer(p, num_lane_change);
 
     reference_path = util::setDecelerationVelocity(
-      *route_handler, reference_path, current_lanes, parameters_.lane_change_prepare_duration,
+      *route_handler, reference_path, current_lanes, parameters_->lane_change_prepare_duration,
       lane_change_buffer);
   }
 
   const auto shorten_lanes = util::cutOverlappedLanes(reference_path, drivable_lanes);
 
   const auto expanded_lanes = util::expandLanelets(
-    shorten_lanes, parameters_.drivable_area_left_bound_offset,
-    parameters_.drivable_area_right_bound_offset, parameters_.drivable_area_types_to_skip);
+    shorten_lanes, parameters_->drivable_area_left_bound_offset,
+    parameters_->drivable_area_right_bound_offset, parameters_->drivable_area_types_to_skip);
 
   util::generateDrivableArea(reference_path, expanded_lanes, p.vehicle_length, planner_data_);