refactor(lane_change): move for lane change module to interface (auto…

…warefoundation#3536) * refactor(lane_change): move for lane change module to interface Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]> * fix build error Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]> * fix conflict --------- Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]> Signed-off-by: Mingyu Li <[email protected]>
Mingyu1991 · Jun 26, 2023 · a8b2fa5 · a8b2fa5
1 parent 1eabe99
commit a8b2fa5
Show file tree

Hide file tree

Showing 14 changed files with 168 additions and 676 deletions.
diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt
@@ -19,9 +19,7 @@ set(common_src
   src/scene_module/goal_planner/goal_planner_module.cpp
   src/scene_module/side_shift/side_shift_module.cpp
   src/scene_module/lane_change/interface.cpp
-  src/scene_module/lane_change/lane_change_module.cpp
   src/scene_module/lane_change/normal.cpp
-  src/scene_module/lane_change/bt_normal.cpp
   src/scene_module/lane_change/external_request.cpp
   src/turn_signal_decider.cpp
   src/utils/utils.cpp

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp
@@ -22,7 +22,6 @@
 #include "behavior_path_planner/behavior_tree_manager.hpp"
 #include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp"
 #include "behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp"
-#include "behavior_path_planner/scene_module/lane_change/lane_change_module.hpp"
 #include "behavior_path_planner/scene_module/lane_following/lane_following_module.hpp"
 #include "behavior_path_planner/scene_module/pull_out/pull_out_module.hpp"
 #include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp"

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
@@ -53,15 +53,19 @@ class LaneChangeBase
 {
 public:
   LaneChangeBase(
-    const std::shared_ptr<LaneChangeParameters> & parameters, LaneChangeModuleType type,
+    std::shared_ptr<LaneChangeParameters> parameters, LaneChangeModuleType type,
     Direction direction)
-  : parameters_{parameters}, direction_{direction}, type_{type}
+  : parameters_{std::move(parameters)}, direction_{direction}, type_{type}
   {
     prev_module_reference_path_ = std::make_shared<PathWithLaneId>();
     prev_module_path_ = std::make_shared<PathWithLaneId>();
     prev_drivable_lanes_ = std::make_shared<std::vector<DrivableLanes>>();
   }
 
+  LaneChangeBase(const LaneChangeBase &) = delete;
+  LaneChangeBase(LaneChangeBase &&) = delete;
+  LaneChangeBase & operator=(const LaneChangeBase &) = delete;
+  LaneChangeBase & operator=(LaneChangeBase &&) = delete;
   virtual ~LaneChangeBase() = default;
 
   virtual void updateLaneChangeStatus() = 0;

diff --git a/...ehavior_path_planner/include/behavior_path_planner/scene_module/lane_change/bt_normal.hpp b/...ehavior_path_planner/include/behavior_path_planner/scene_module/lane_change/bt_normal.hpp
diff --git a/..._path_planner/include/behavior_path_planner/scene_module/lane_change/external_request.hpp b/..._path_planner/include/behavior_path_planner/scene_module/lane_change/external_request.hpp
@@ -15,7 +15,6 @@
 #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__EXTERNAL_REQUEST_HPP_
 #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__EXTERNAL_REQUEST_HPP_
 
-#include "behavior_path_planner/scene_module/lane_change/bt_normal.hpp"
 #include "behavior_path_planner/scene_module/lane_change/normal.hpp"
 
 #include <memory>

diff --git a/...ehavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp b/...ehavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp
@@ -142,6 +142,26 @@ class LaneChangeBTInterface : public LaneChangeInterface
   bool is_activated_{false};
 };
 
+class LaneChangeBTModule : public LaneChangeBTInterface
+{
+public:
+  LaneChangeBTModule(
+    const std::string & name, rclcpp::Node & node,
+    const std::shared_ptr<LaneChangeParameters> & parameters);
+
+protected:
+  void updateRTCStatus(const double start_distance, const double finish_distance) override
+  {
+    const auto direction = std::invoke([&]() -> std::string {
+      const auto dir = module_type_->getDirection();
+      return (dir == Direction::LEFT) ? "left" : "right";
+    });
+
+    rtc_interface_ptr_map_.at(direction)->updateCooperateStatus(
+      uuid_map_.at(direction), isExecutionReady(), start_distance, finish_distance, clock_->now());
+  }
+};
+
 class ExternalRequestLaneChangeLeftBTModule : public LaneChangeBTInterface
 {
 public:

diff --git a/...ath_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp b/...ath_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp
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
@@ -39,6 +39,10 @@ class NormalLaneChange : public LaneChangeBase
     const std::shared_ptr<LaneChangeParameters> & parameters, LaneChangeModuleType type,
     Direction direction);
 
+  NormalLaneChange(const NormalLaneChange &) = delete;
+  NormalLaneChange(NormalLaneChange &&) = delete;
+  NormalLaneChange & operator=(const NormalLaneChange &) = delete;
+  NormalLaneChange & operator=(NormalLaneChange &&) = delete;
   ~NormalLaneChange() override = default;
 
   void updateLaneChangeStatus() override;
@@ -86,5 +90,33 @@ class NormalLaneChange : public LaneChangeBase
 
   bool isValidPath(const PathWithLaneId & path) const override;
 };
+
+class NormalLaneChangeBT : public NormalLaneChange
+{
+public:
+  NormalLaneChangeBT(
+    const std::shared_ptr<LaneChangeParameters> & parameters, LaneChangeModuleType type,
+    Direction direction);
+
+  NormalLaneChangeBT(const NormalLaneChangeBT &) = delete;
+  NormalLaneChangeBT(NormalLaneChangeBT &&) = delete;
+  NormalLaneChangeBT & operator=(const NormalLaneChangeBT &) = delete;
+  NormalLaneChangeBT & operator=(NormalLaneChangeBT &&) = delete;
+  ~NormalLaneChangeBT() override = default;
+
+  PathWithLaneId getReferencePath() const override;
+
+protected:
+  lanelet::ConstLanelets getCurrentLanes() const override;
+
+  int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const override;
+
+  PathWithLaneId getPrepareSegment(
+    const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current,
+    const double backward_path_length, const double prepare_length,
+    const double prepare_velocity) const override;
+
+  std::vector<DrivableLanes> getDrivableLanes() const override;
+};
 }  // namespace behavior_path_planner
 #endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__NORMAL_HPP_
diff --git a/...havior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/...havior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp
@@ -390,14 +390,14 @@ class SceneModuleInterface
     for (const auto & rtc_type : rtc_types) {
       const auto snake_case_name = utils::convertToSnakeCase(name);
       const auto rtc_interface_name =
-        rtc_type == "" ? snake_case_name : snake_case_name + "_" + rtc_type;
+        rtc_type.empty() ? snake_case_name : (snake_case_name + "_" + rtc_type);
       rtc_interface_ptr_map.emplace(
         rtc_type, std::make_shared<RTCInterface>(&node, rtc_interface_name));
     }
     return rtc_interface_ptr_map;
   }
 
-  void updateRTCStatus(const double start_distance, const double finish_distance)
+  virtual void updateRTCStatus(const double start_distance, const double finish_distance)
   {
     for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) {
       if (itr->second) {

diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -172,7 +172,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
     bt_manager_->registerSceneModule(ext_request_lane_change_left_module);
 
     auto lane_change_module =
-      std::make_shared<LaneChangeModule>("LaneChange", *this, lane_change_param_ptr_);
+      std::make_shared<LaneChangeBTModule>("LaneChange", *this, lane_change_param_ptr_);
     path_candidate_publishers_.emplace(
       "LaneChange", create_publisher<Path>(path_candidate_name_space + "lane_change", 1));
     bt_manager_->registerSceneModule(lane_change_module);