refactor(lane change): lane change class rework (#3060)

* refactor(lane change): lane change class rework

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

* fix lane change module search

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

* Update planning/route_handler/include/route_handler/route_handler.hpp

Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>

* Update planning/route_handler/include/route_handler/route_handler.hpp

Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>

* set the COMPILE_WITH_OLD_ARCHITECTURE to true

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

---------

Signed-off-by: Muhammad Zulfaqar <zulfaqar.azmi@tier4.jp>
Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>

planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp

@@ -33,7 +33,10 @@ struct BehaviorPathPlannerParameters
   ModuleConfigParameters config_pull_out;
   ModuleConfigParameters config_pull_over;
   ModuleConfigParameters config_side_shift;
-  ModuleConfigParameters config_lane_change;
+  ModuleConfigParameters config_lane_change_left;
+  ModuleConfigParameters config_lane_change_right;
+  ModuleConfigParameters config_external_lane_change_left;
+  ModuleConfigParameters config_external_lane_change_right;
 
   double backward_path_length;
   double forward_path_length;

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

@@ -45,6 +45,7 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Twist;
 using marker_utils::CollisionCheckDebug;
+using route_handler::Direction;
 using tier4_planning_msgs::msg::LaneChangeDebugMsg;
 using tier4_planning_msgs::msg::LaneChangeDebugMsgArray;
 
@@ -59,8 +60,7 @@ class LaneChangeModule : public SceneModuleInterface
   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);
+    const std::shared_ptr<RTCInterface> & rtc_interface, Direction direction);
 #endif
   bool isExecutionRequested() const override;
   bool isExecutionReady() const override;
@@ -75,13 +75,7 @@ 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
-
+#ifdef USE_OLD_ARCHITECTURE
   void publishRTCStatus() override
   {
     rtc_interface_left_->publishCooperateStatus(clock_->now());
@@ -110,6 +104,12 @@ class LaneChangeModule : public SceneModuleInterface
     rtc_interface_left_->unlockCommandUpdate();
     rtc_interface_right_->unlockCommandUpdate();
   }
+#else
+  void updateModuleParams(const std::shared_ptr<LaneChangeParameters> & parameters)
+  {
+    parameters_ = parameters;
+  }
+#endif
 
 private:
   std::shared_ptr<LaneChangeParameters> parameters_;
@@ -125,16 +125,21 @@ class LaneChangeModule : public SceneModuleInterface
   bool is_abort_path_approved_{false};
   bool is_abort_approval_requested_{false};
   bool is_abort_condition_satisfied_{false};
-  bool is_activated_ = false;
+  bool is_activated_{false};
 
+#ifdef USE_OLD_ARCHITECTURE
   std::shared_ptr<RTCInterface> rtc_interface_left_;
   std::shared_ptr<RTCInterface> rtc_interface_right_;
   UUID uuid_left_;
   UUID uuid_right_;
   UUID candidate_uuid_;
+#else
+  Direction direction_{Direction::NONE};
+#endif
 
   void resetParameters();
 
+#ifdef USE_OLD_ARCHITECTURE
   void waitApprovalLeft(const double start_distance, const double finish_distance)
   {
     rtc_interface_left_->updateCooperateStatus(
@@ -190,8 +195,8 @@ class LaneChangeModule : public SceneModuleInterface
       rtc_interface_right_->removeCooperateStatus(uuid_right_);
     }
   }
+#endif
 
-  lanelet::ConstLanelets get_original_lanes() const;
   PathWithLaneId getReferencePath() const;
   lanelet::ConstLanelets getLaneChangeLanes(
     const lanelet::ConstLanelets & current_lanes, const double lane_change_lane_length) const;

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

@@ -33,23 +33,24 @@ class LaneChangeModuleManager : public SceneModuleManagerInterface
 public:
   LaneChangeModuleManager(
     rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config,
-    std::shared_ptr<LaneChangeParameters> parameters);
+    std::shared_ptr<LaneChangeParameters> parameters, Direction direction);
 
   std::shared_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
   {
     return std::make_shared<LaneChangeModule>(
-      name_, *node_, parameters_, rtc_interface_left_, rtc_interface_right_);
+      name_, *node_, parameters_, rtc_interface_, direction_);
   }
 
   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<RTCInterface> rtc_interface_;
 
   std::shared_ptr<LaneChangeParameters> parameters_;
 
   std::vector<std::shared_ptr<LaneChangeModule>> registered_modules_;
+
+  Direction direction_;
 };
 
 }  // namespace behavior_path_planner

planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/lane_change_module_data.hpp

@@ -79,6 +79,11 @@ struct LaneChangeTargetObjectIndices
   std::vector<size_t> target_lane{};
   std::vector<size_t> other_lane{};
 };
+
+enum class LaneChangeType {
+  Normal = 0,
+  ExternalRequest = 1,
+};
 }  // namespace behavior_path_planner
 
 #endif  // BEHAVIOR_PATH_PLANNER__UTIL__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_

planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp

@@ -45,6 +45,7 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Twist;
 using marker_utils::CollisionCheckDebug;
+using route_handler::Direction;
 using tier4_autoware_utils::Polygon2d;
 
 PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2);
@@ -162,5 +163,7 @@ LaneChangeTargetObjectIndices filterObjectIndices(
 
 double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer = 0.0);
 
+std::string getStrDirection(const std::string name, const Direction direction);
+
 }  // namespace behavior_path_planner::lane_change_utils
 #endif  // BEHAVIOR_PATH_PLANNER__UTIL__LANE_CHANGE__UTIL_HPP_

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -234,10 +234,23 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
         "side_shift", create_publisher<Path>(path_reference_name_space + "side_shift", 1));
     }
 
-    if (p.config_lane_change.enable_module) {
-      const std::string module_topic = "lane_change";
+    if (p.config_lane_change_left.enable_module) {
+      const std::string module_topic = "lane_change_left";
       auto manager = std::make_shared<LaneChangeModuleManager>(
-        this, module_topic, p.config_lane_change, lane_change_param_ptr_);
+        this, module_topic, p.config_lane_change_left, lane_change_param_ptr_,
+        route_handler::Direction::LEFT);
+      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));
+    }
+
+    if (p.config_lane_change_right.enable_module) {
+      const std::string module_topic = "lane_change_right";
+      auto manager = std::make_shared<LaneChangeModuleManager>(
+        this, module_topic, p.config_lane_change_right, lane_change_param_ptr_,
+        route_handler::Direction::RIGHT);
       planner_manager_->registerSceneModuleManager(manager);
       path_candidate_publishers_.emplace(
         module_topic, create_publisher<Path>(path_candidate_name_space + module_topic, 1));
@@ -316,12 +329,21 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
   }
 
   {
-    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 =
+    const std::string ns = "lane_change_left.";
+    p.config_lane_change_left.enable_module = declare_parameter<bool>(ns + "enable_module");
+    p.config_lane_change_left.enable_simultaneous_execution =
+      declare_parameter<bool>(ns + "enable_simultaneous_execution");
+    p.config_lane_change_left.priority = declare_parameter<int>(ns + "priority");
+    p.config_lane_change_left.max_module_size = declare_parameter<int>(ns + "max_module_size");
+  }
+
+  {
+    const std::string ns = "lane_change_right.";
+    p.config_lane_change_right.enable_module = declare_parameter<bool>(ns + "enable_module");
+    p.config_lane_change_right.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");
+    p.config_lane_change_right.priority = declare_parameter<int>(ns + "priority");
+    p.config_lane_change_right.max_module_size = declare_parameter<int>(ns + "max_module_size");
   }
 
   {

planning/behavior_path_planner/src/planner_manager.cpp

@@ -281,7 +281,7 @@ BehaviorModuleOutput PlannerManager::update(const std::shared_ptr<PlannerData> &
     if ((*itr)->getCurrentStatus() != ModuleStatus::RUNNING) {
       if (itr == approved_module_ptrs_.begin()) {
         // update root lanelet when the lane change is done.
-        if (name == "lane_change") {
+        if (name.find("lane_change") != std::string::npos) {
           root_lanelet_ = updateRootLanelet(data);
         }
 

planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp

@@ -31,15 +31,6 @@
 #include <utility>
 #include <vector>
 
-std::string to_string(const unique_identifier_msgs::msg::UUID & uuid)
-{
-  std::stringstream ss;
-  for (auto i = 0; i < 16; ++i) {
-    ss << std::hex << std::setfill('0') << std::setw(2) << +uuid.uuid[i];
-  }
-  return ss.str();
-}
-
 namespace behavior_path_planner
 {
 using autoware_auto_perception_msgs::msg::ObjectClassification;
@@ -60,15 +51,10 @@ LaneChangeModule::LaneChangeModule(
 LaneChangeModule::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)
-: SceneModuleInterface{name, node},
-  parameters_{parameters},
-  rtc_interface_left_{rtc_interface_left},
-  rtc_interface_right_{rtc_interface_right},
-  uuid_left_{generateUUID()},
-  uuid_right_{generateUUID()}
+  const std::shared_ptr<RTCInterface> & rtc_interface, Direction direction)
+: SceneModuleInterface{name, node}, parameters_{parameters}, direction_{direction}
 {
+  rtc_interface_ptr_ = rtc_interface;
   steering_factor_interface_ptr_ = std::make_unique<SteeringFactorInterface>(&node, "lane_change");
 }
 #endif
@@ -107,6 +93,10 @@ bool LaneChangeModule::isExecutionRequested() const
 #endif
   const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_);
 
+  if (lane_change_lanes.empty()) {
+    return false;
+  }
+
   LaneChangePath selected_path;
   const auto [found_valid_path, found_safe_path] =
     getSafePath(lane_change_lanes, check_distance_, selected_path);
@@ -128,6 +118,10 @@ bool LaneChangeModule::isExecutionReady() const
 #endif
   const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_);
 
+  if (lane_change_lanes.empty()) {
+    return false;
+  }
+
   LaneChangePath selected_path;
   const auto [found_valid_path, found_safe_path] =
     getSafePath(lane_change_lanes, check_distance_, selected_path);
@@ -218,6 +212,7 @@ BehaviorModuleOutput LaneChangeModule::plan()
 void LaneChangeModule::resetPathIfAbort()
 {
   if (!is_abort_approval_requested_) {
+#ifdef USE_OLD_ARCHITECTURE
     const auto lateral_shift = lane_change_utils::getLateralShift(*abort_path_);
     if (lateral_shift > 0.0) {
       removePreviousRTCStatusRight();
@@ -226,6 +221,9 @@ void LaneChangeModule::resetPathIfAbort()
       removePreviousRTCStatusLeft();
       uuid_left_ = generateUUID();
     }
+#else
+    removeRTCStatus();
+#endif
     RCLCPP_DEBUG(getLogger(), "[abort] uuid is reset to request abort approval.");
     is_abort_approval_requested_ = true;
     is_abort_path_approved_ = false;
@@ -257,6 +255,10 @@ CandidateOutput LaneChangeModule::planCandidate() const
 #endif
   const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_);
 
+  if (lane_change_lanes.empty()) {
+    return output;
+  }
+
 #ifdef USE_OLD_ARCHITECTURE
   [[maybe_unused]] const auto [found_valid_path, found_safe_path] =
     getSafePath(lane_change_lanes, check_distance_, selected_path);
@@ -308,9 +310,15 @@ BehaviorModuleOutput LaneChangeModule::planWaitingApproval()
   const auto candidate = planCandidate();
   path_candidate_ = std::make_shared<PathWithLaneId>(candidate.path_candidate);
   path_reference_ = getPreviousModuleOutput().reference_path;
+#ifdef USE_OLD_ARCHITECTURE
   updateRTCStatus(candidate);
   waitApproval();
+#else
+  updateRTCStatus(
+    candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change);
+#endif
   is_abort_path_approved_ = false;
+
   return out;
 }
 
@@ -416,7 +424,11 @@ lanelet::ConstLanelets LaneChangeModule::getLaneChangeLanes(
   lanelet::ConstLanelets current_check_lanes =
     route_handler->getLaneletSequence(current_lane, current_pose, 0.0, lane_change_prepare_length);
   lanelet::ConstLanelet lane_change_lane;
+#ifdef USE_OLD_ARCHITECTURE
   if (route_handler->getLaneChangeTarget(current_check_lanes, &lane_change_lane)) {
+#else
+  if (route_handler->getLaneChangeTarget(current_check_lanes, &lane_change_lane, direction_)) {
+#endif
     lane_change_lanes = route_handler->getLaneletSequence(
       lane_change_lane, current_pose, lane_change_lane_length, lane_change_lane_length);
   } else {
@@ -667,13 +679,14 @@ std::shared_ptr<LaneChangeDebugMsgArray> LaneChangeModule::get_debug_msg_array()
 
 void LaneChangeModule::updateSteeringFactorPtr(const BehaviorModuleOutput & output)
 {
-  const auto turn_signal_info = output.turn_signal_info;
   const auto current_pose = getEgoPose();
   const double start_distance = motion_utils::calcSignedArcLength(
     output.path->points, current_pose.position, status_.lane_change_path.shift_line.start.position);
   const double finish_distance = motion_utils::calcSignedArcLength(
     output.path->points, current_pose.position, status_.lane_change_path.shift_line.end.position);
 
+#ifdef USE_OLD_ARCHITECTURE
+  const auto turn_signal_info = output.turn_signal_info;
   const uint16_t steering_factor_direction =
     std::invoke([this, &start_distance, &finish_distance, &turn_signal_info]() {
       if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) {
@@ -686,6 +699,17 @@ void LaneChangeModule::updateSteeringFactorPtr(const BehaviorModuleOutput & outp
       }
       return SteeringFactor::UNKNOWN;
     });
+#else
+  const auto steering_factor_direction = std::invoke([&]() {
+    if (direction_ == Direction::LEFT) {
+      return SteeringFactor::LEFT;
+    }
+    if (direction_ == Direction::RIGHT) {
+      return SteeringFactor::RIGHT;
+    }
+    return SteeringFactor::UNKNOWN;
+  });
+#endif
 
   // TODO(tkhmy) add handle status TRYING
   steering_factor_interface_ptr_->updateSteeringFactor(

planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp

@@ -23,14 +23,15 @@
 
 namespace behavior_path_planner
 {
-
+using route_handler::Direction;
 LaneChangeModuleManager::LaneChangeModuleManager(
   rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config,
-  std::shared_ptr<LaneChangeParameters> parameters)
-: SceneModuleManagerInterface(node, name, config), parameters_{std::move(parameters)}
+  std::shared_ptr<LaneChangeParameters> parameters, Direction direction)
+: SceneModuleManagerInterface(node, name, config),
+  parameters_{std::move(parameters)},
+  direction_{direction}
 {
-  rtc_interface_left_ = std::make_shared<RTCInterface>(node, name + "_left");
-  rtc_interface_right_ = std::make_shared<RTCInterface>(node, name + "_right");
+  rtc_interface_ = std::make_shared<RTCInterface>(node, name);
 }
 
 void LaneChangeModuleManager::updateModuleParams(
@@ -40,7 +41,7 @@ void LaneChangeModuleManager::updateModuleParams(
 
   [[maybe_unused]] auto p = parameters_;
 
-  [[maybe_unused]] std::string ns = "lane_change.";
+  [[maybe_unused]] const std::string ns = name_ + ".";
   // updateParam<bool>(parameters, ns + ..., ...);
 
   std::for_each(registered_modules_.begin(), registered_modules_.end(), [&p](const auto & m) {