feat(lane_change): support new framework

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

planning/behavior_path_planner/CMakeLists.txt

@@ -7,14 +7,15 @@ autoware_package()
 find_package(OpenCV REQUIRED)
 find_package(magic_enum CONFIG REQUIRED)
 
-set(COMPILE_WITH_OLD_ARCHITECTURE TRUE)
+set(COMPILE_WITH_OLD_ARCHITECTURE FALSE)
 
 set(common_src
   src/utilities.cpp
   src/path_utilities.cpp
   src/steering_factor_interface.cpp
   src/behavior_path_planner_node.cpp
   src/scene_module/scene_module_visitor.cpp
+  src/scene_module/lane_change/lane_change_module.cpp
   src/turn_signal_decider.cpp
   src/util/avoidance/util.cpp
   src/util/lane_change/util.cpp
@@ -47,7 +48,6 @@ if(COMPILE_WITH_OLD_ARCHITECTURE)
     src/scene_module/avoidance/avoidance_module.cpp
     src/scene_module/lane_following/lane_following_module.cpp
     src/scene_module/lane_change/external_request_lane_change_module.cpp
-    src/scene_module/lane_change/lane_change_module.cpp
     src/scene_module/pull_over/pull_over_module.cpp
     src/scene_module/pull_out/pull_out_module.cpp
     ${common_src}
@@ -60,6 +60,7 @@ if(COMPILE_WITH_OLD_ARCHITECTURE)
 else()
   ament_auto_add_library(behavior_path_planner_node SHARED
     src/planner_manager.cpp
+    src/scene_module/lane_change/manager.cpp
     ${common_src}
   )
 

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -29,6 +29,7 @@
 #include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp"
 #else
 #include "behavior_path_planner/planner_manager.hpp"
+#include "behavior_path_planner/scene_module/lane_change/manager.hpp"
 #endif
 
 #include "behavior_path_planner/steering_factor_interface.hpp"

planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp

@@ -30,6 +30,7 @@ struct BehaviorPathPlannerParameters
   bool verbose;
 
   ModuleConfigParameters config_avoidance;
+  ModuleConfigParameters config_lane_change;
 
   double backward_path_length;
   double forward_path_length;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp

@@ -51,13 +51,20 @@ using tier4_planning_msgs::msg::LaneChangeDebugMsgArray;
 class LaneChangeModule : public SceneModuleInterface
 {
 public:
+#ifdef USE_OLD_ARCHITECTURE
   LaneChangeModule(
     const std::string & name, rclcpp::Node & node,
     std::shared_ptr<LaneChangeParameters> parameters);
-
+#else
+  LaneChangeModule(
+    const std::string & name, rclcpp::Node & node,
+    const std::shared_ptr<LaneChangeParameters> & parameters,
+    const std::shared_ptr<RTCInterface> & rtc_interface_left,
+    const std::shared_ptr<RTCInterface> & rtc_interface_right);
+#endif
   bool isExecutionRequested() const override;
   bool isExecutionReady() const override;
-  BT::NodeStatus updateState() override;
+  ModuleStatus updateState() override;
   BehaviorModuleOutput plan() override;
   BehaviorModuleOutput planWaitingApproval() override;
   CandidateOutput planCandidate() const override;
@@ -68,33 +75,40 @@ class LaneChangeModule : public SceneModuleInterface
   void acceptVisitor(
     [[maybe_unused]] const std::shared_ptr<SceneModuleVisitor> & visitor) const override;
 
+#ifndef USE_OLD_ARCHITECTURE
+  void updateModuleParams(const std::shared_ptr<LaneChangeParameters> & parameters)
+  {
+    parameters_ = parameters;
+  }
+#endif
+
   void publishRTCStatus() override
   {
-    rtc_interface_left_.publishCooperateStatus(clock_->now());
-    rtc_interface_right_.publishCooperateStatus(clock_->now());
+    rtc_interface_left_->publishCooperateStatus(clock_->now());
+    rtc_interface_right_->publishCooperateStatus(clock_->now());
   }
 
   bool isActivated() override
   {
-    if (rtc_interface_left_.isRegistered(uuid_left_)) {
-      return rtc_interface_left_.isActivated(uuid_left_);
+    if (rtc_interface_left_->isRegistered(uuid_left_)) {
+      return rtc_interface_left_->isActivated(uuid_left_);
     }
-    if (rtc_interface_right_.isRegistered(uuid_right_)) {
-      return rtc_interface_right_.isActivated(uuid_right_);
+    if (rtc_interface_right_->isRegistered(uuid_right_)) {
+      return rtc_interface_right_->isActivated(uuid_right_);
     }
     return false;
   }
 
   void lockRTCCommand() override
   {
-    rtc_interface_left_.lockCommandUpdate();
-    rtc_interface_right_.lockCommandUpdate();
+    rtc_interface_left_->lockCommandUpdate();
+    rtc_interface_right_->lockCommandUpdate();
   }
 
   void unlockRTCCommand() override
   {
-    rtc_interface_left_.unlockCommandUpdate();
-    rtc_interface_right_.unlockCommandUpdate();
+    rtc_interface_left_->unlockCommandUpdate();
+    rtc_interface_right_->unlockCommandUpdate();
   }
 
 private:
@@ -113,8 +127,8 @@ class LaneChangeModule : public SceneModuleInterface
   bool is_abort_condition_satisfied_{false};
   bool is_activated_ = false;
 
-  RTCInterface rtc_interface_left_;
-  RTCInterface rtc_interface_right_;
+  std::shared_ptr<RTCInterface> rtc_interface_left_;
+  std::shared_ptr<RTCInterface> rtc_interface_right_;
   UUID uuid_left_;
   UUID uuid_right_;
   UUID candidate_uuid_;
@@ -123,29 +137,29 @@ class LaneChangeModule : public SceneModuleInterface
 
   void waitApprovalLeft(const double start_distance, const double finish_distance)
   {
-    rtc_interface_left_.updateCooperateStatus(
+    rtc_interface_left_->updateCooperateStatus(
       uuid_left_, isExecutionReady(), start_distance, finish_distance, clock_->now());
     is_waiting_approval_ = true;
   }
 
   void waitApprovalRight(const double start_distance, const double finish_distance)
   {
-    rtc_interface_right_.updateCooperateStatus(
+    rtc_interface_right_->updateCooperateStatus(
       uuid_right_, isExecutionReady(), start_distance, finish_distance, clock_->now());
     is_waiting_approval_ = true;
   }
 
   void updateRTCStatus(const CandidateOutput & candidate)
   {
     if (candidate.lateral_shift > 0.0) {
-      rtc_interface_left_.updateCooperateStatus(
+      rtc_interface_left_->updateCooperateStatus(
         uuid_left_, isExecutionReady(), candidate.start_distance_to_path_change,
         candidate.finish_distance_to_path_change, clock_->now());
       candidate_uuid_ = uuid_left_;
       return;
     }
     if (candidate.lateral_shift < 0.0) {
-      rtc_interface_right_.updateCooperateStatus(
+      rtc_interface_right_->updateCooperateStatus(
         uuid_right_, isExecutionReady(), candidate.start_distance_to_path_change,
         candidate.finish_distance_to_path_change, clock_->now());
       candidate_uuid_ = uuid_right_;
@@ -159,21 +173,21 @@ class LaneChangeModule : public SceneModuleInterface
 
   void removeRTCStatus() override
   {
-    rtc_interface_left_.clearCooperateStatus();
-    rtc_interface_right_.clearCooperateStatus();
+    rtc_interface_left_->clearCooperateStatus();
+    rtc_interface_right_->clearCooperateStatus();
   }
 
   void removePreviousRTCStatusLeft()
   {
-    if (rtc_interface_left_.isRegistered(uuid_left_)) {
-      rtc_interface_left_.removeCooperateStatus(uuid_left_);
+    if (rtc_interface_left_->isRegistered(uuid_left_)) {
+      rtc_interface_left_->removeCooperateStatus(uuid_left_);
     }
   }
 
   void removePreviousRTCStatusRight()
   {
-    if (rtc_interface_right_.isRegistered(uuid_right_)) {
-      rtc_interface_right_.removeCooperateStatus(uuid_right_);
+    if (rtc_interface_right_->isRegistered(uuid_right_)) {
+      rtc_interface_right_->removeCooperateStatus(uuid_right_);
     }
   }
 
@@ -184,6 +198,7 @@ class LaneChangeModule : public SceneModuleInterface
   std::pair<bool, bool> getSafePath(
     const lanelet::ConstLanelets & lane_change_lanes, const double check_distance,
     LaneChangePath & safe_path) const;
+  PathWithLaneId extendBackwardLength(const PathWithLaneId & original_path) const;
 
   void updateLaneChangeStatus();
   void generateExtendedDrivableArea(PathWithLaneId & path);

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp

@@ -0,0 +1,57 @@
+// 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__SCENE_MODULE__LANE_CHANGE__MANAGER_HPP_
+#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__MANAGER_HPP_
+
+#include "behavior_path_planner/scene_module/lane_change/lane_change_module.hpp"
+#include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace behavior_path_planner
+{
+
+class LaneChangeModuleManager : public SceneModuleManagerInterface
+{
+public:
+  LaneChangeModuleManager(
+    rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config,
+    std::shared_ptr<LaneChangeParameters> parameters);
+
+  std::shared_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
+  {
+    return std::make_shared<LaneChangeModule>(
+      name_, *node_, parameters_, rtc_interface_left_, rtc_interface_right_);
+  }
+
+  void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;
+
+private:
+  std::shared_ptr<RTCInterface> rtc_interface_left_;
+  std::shared_ptr<RTCInterface> rtc_interface_right_;
+
+  std::shared_ptr<LaneChangeParameters> parameters_;
+
+  std::vector<std::shared_ptr<LaneChangeModule>> registered_modules_;
+};
+
+}  // namespace behavior_path_planner
+
+#endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__MANAGER_HPP_

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -197,6 +197,17 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
     planner_manager_ =
       std::make_shared<PlannerManager>(*this, lane_following_param_ptr_, p.verbose);
 
+    if (p.config_lane_change.enable_module) {
+      const std::string module_topic = "lane_change";
+      auto manager = std::make_shared<LaneChangeModuleManager>(
+        this, module_topic, p.config_lane_change, lane_change_param_ptr_);
+      planner_manager_->registerSceneModuleManager(manager);
+      path_candidate_publishers_.emplace(
+        module_topic, create_publisher<Path>(path_candidate_name_space + module_topic, 1));
+      path_reference_publishers_.emplace(
+        module_topic, create_publisher<Path>(path_reference_name_space + module_topic, 1));
+    }
+
     mutex_bt_.unlock();
   }
 #endif
@@ -229,7 +240,15 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
   BehaviorPathPlannerParameters p{};
 
   p.verbose = declare_parameter<bool>("verbose");
+  {
+    const std::string ns = "lane_change.";
 
+    p.config_lane_change.enable_module = declare_parameter<bool>(ns + "enable_module");
+    p.config_lane_change.enable_simultaneous_execution =
+      declare_parameter<bool>(ns + "enable_simultaneous_execution");
+    p.config_lane_change.priority = declare_parameter<int>(ns + "priority");
+    p.config_lane_change.max_module_size = declare_parameter<int>(ns + "max_module_size");
+  }
   // vehicle info
   const auto vehicle_info = VehicleInfoUtil(*this).getVehicleInfo();
   p.vehicle_info = vehicle_info;