refactor(avoidance_by_lane_change): reuse lane change class (#3577)

* refactor(avoidance_by_lane_change): reuse lane change class Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * fix wrong direction Signed-off-by: Muhammad Zulfaqar <zulfaqar.azmi@tier4.jp> * fix conflict Signed-off-by: Muhammad Zulfaqar <zulfaqar.azmi@tier4.jp> * fix rtc paramters Signed-off-by: Muhammad Zulfaqar <zulfaqar.azmi@tier4.jp> * Revert "fix rtc paramters" This reverts commit 16be270. * fix rtc Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * fix pre-commit Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * fix pre-commit and move updateRTCStatus to cpp file Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * COMPILE_WITH_OLD_ARCHITECTURE=TRUE Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> --------- Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> Signed-off-by: Muhammad Zulfaqar <zulfaqar.azmi@tier4.jp>
autowarefoundation · May 8, 2023 · de8afd7 · de8afd7
1 parent db8e81e
commit de8afd7
Show file tree

Hide file tree

Showing 17 changed files with 487 additions and 723 deletions.
diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt
@@ -14,14 +14,14 @@ set(common_src
   src/behavior_path_planner_node.cpp
   src/scene_module/scene_module_visitor.cpp
   src/scene_module/avoidance/avoidance_module.cpp
-  src/scene_module/avoidance_by_lc/module.cpp
   src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp
   src/scene_module/pull_out/pull_out_module.cpp
   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/normal.cpp
   src/scene_module/lane_change/external_request.cpp
+  src/scene_module/lane_change/avoidance_by_lane_change.cpp
   src/turn_signal_decider.cpp
   src/utils/utils.cpp
   src/utils/path_utils.cpp
@@ -66,7 +66,6 @@ else()
   ament_auto_add_library(behavior_path_planner_node SHARED
     src/planner_manager.cpp
     src/scene_module/avoidance/manager.cpp
-    src/scene_module/avoidance_by_lc/manager.cpp
     src/scene_module/dynamic_avoidance/manager.cpp
     src/scene_module/pull_out/manager.cpp
     src/scene_module/goal_planner/manager.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
@@ -29,7 +29,6 @@
 #else
 #include "behavior_path_planner/planner_manager.hpp"
 #include "behavior_path_planner/scene_module/avoidance/manager.hpp"
-#include "behavior_path_planner/scene_module/avoidance_by_lc/manager.hpp"
 #include "behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp"
 #include "behavior_path_planner/scene_module/goal_planner/manager.hpp"
 #include "behavior_path_planner/scene_module/lane_change/manager.hpp"
@@ -40,7 +39,6 @@
 #include "behavior_path_planner/steering_factor_interface.hpp"
 #include "behavior_path_planner/turn_signal_decider.hpp"
 #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp"
-#include "behavior_path_planner/utils/avoidance_by_lc/module_data.hpp"
 #include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp"
 #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp"
 #include "behavior_path_planner/utils/lane_following/module_data.hpp"

diff --git a/...anner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp b/...anner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp
@@ -0,0 +1,62 @@
+// 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__AVOIDANCE_BY_LANE_CHANGE_HPP_
+#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__AVOIDANCE_BY_LANE_CHANGE_HPP_
+
+#include "behavior_path_planner/scene_module/lane_change/normal.hpp"
+
+#include <memory>
+#include <utility>
+
+namespace behavior_path_planner
+{
+using autoware_auto_planning_msgs::msg::PathWithLaneId;
+using geometry_msgs::msg::Pose;
+using geometry_msgs::msg::Twist;
+using tier4_planning_msgs::msg::LaneChangeDebugMsg;
+using tier4_planning_msgs::msg::LaneChangeDebugMsgArray;
+using AvoidanceDebugData = DebugData;
+
+class AvoidanceByLaneChange : public NormalLaneChange
+{
+public:
+  AvoidanceByLaneChange(
+    const std::shared_ptr<LaneChangeParameters> & parameters,
+    std::shared_ptr<AvoidanceParameters> avoidance_parameters,
+    std::shared_ptr<AvoidanceByLCParameters> avoidance_by_lane_change_parameters);
+
+  void updateSpecialData() override;
+
+  std::pair<bool, bool> getSafePath(LaneChangePath & safe_path) const override;
+
+private:
+  std::shared_ptr<AvoidanceParameters> avoidance_parameters_;
+  std::shared_ptr<AvoidanceByLCParameters> avoidance_by_lane_change_parameters_;
+
+  AvoidancePlanningData calcAvoidancePlanningData(AvoidanceDebugData & debug) const;
+  AvoidancePlanningData avoidance_data_;
+  mutable AvoidanceDebugData avoidance_debug_data_;
+
+  ObjectDataArray registered_objects_;
+  mutable ObjectDataArray stopped_objects_;
+
+  ObjectData createObjectData(
+    const AvoidancePlanningData & data, const PredictedObject & object) const;
+
+  void fillAvoidanceTargetObjects(AvoidancePlanningData & data, AvoidanceDebugData & debug) const;
+};
+}  // namespace behavior_path_planner
+
+#endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__AVOIDANCE_BY_LANE_CHANGE_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
@@ -55,7 +55,7 @@ class LaneChangeBase
   LaneChangeBase(
     std::shared_ptr<LaneChangeParameters> parameters, LaneChangeModuleType type,
     Direction direction)
-  : parameters_{std::move(parameters)}, direction_{direction}, type_{type}
+  : lane_change_parameters_{std::move(parameters)}, direction_{direction}, type_{type}
   {
     prev_module_reference_path_ = std::make_shared<PathWithLaneId>();
     prev_module_path_ = std::make_shared<PathWithLaneId>();
@@ -107,6 +107,8 @@ class LaneChangeBase
     }
   }
 
+  virtual void updateSpecialData() {}
+
   const LaneChangeStatus & getLaneChangeStatus() const { return status_; }
 
   LaneChangePath getLaneChangePath() const
@@ -120,7 +122,7 @@ class LaneChangeBase
 
   bool isAbortState() const
   {
-    if (!parameters_->enable_abort_lane_change) {
+    if (!lane_change_parameters_->enable_abort_lane_change) {
       return false;
     }
 
@@ -167,7 +169,7 @@ class LaneChangeBase
 
   Direction getDirection() const
   {
-    if (direction_ == Direction::NONE) {
+    if (direction_ == Direction::NONE && !status_.lane_change_path.path.points.empty()) {
       const auto lateral_shift = utils::lane_change::getLateralShift(status_.lane_change_path);
       return lateral_shift > 0.0 ? Direction::LEFT : Direction::RIGHT;
     }
@@ -187,7 +189,8 @@ class LaneChangeBase
 
   virtual bool getLaneChangePaths(
     const lanelet::ConstLanelets & original_lanelets,
-    const lanelet::ConstLanelets & target_lanelets, LaneChangePaths * candidate_paths) const = 0;
+    const lanelet::ConstLanelets & target_lanelets, Direction direction,
+    LaneChangePaths * candidate_paths) const = 0;
 
   virtual std::vector<DrivableLanes> getDrivableLanes() const = 0;
 
@@ -197,7 +200,8 @@ class LaneChangeBase
 
   virtual bool isValidPath(const PathWithLaneId & path) const = 0;
 
-  lanelet::ConstLanelets getLaneChangeLanes(const lanelet::ConstLanelets & current_lanes) const;
+  virtual lanelet::ConstLanelets getLaneChangeLanes(
+    const lanelet::ConstLanelets & current_lanes, Direction direction) const = 0;
 
   bool isNearEndOfLane() const
   {
@@ -222,7 +226,7 @@ class LaneChangeBase
 
   LaneChangeStates current_lane_change_state_{};
 
-  std::shared_ptr<LaneChangeParameters> parameters_{};
+  std::shared_ptr<LaneChangeParameters> lane_change_parameters_{};
   std::shared_ptr<LaneChangePath> abort_path_{};
   std::shared_ptr<const PlannerData> planner_data_{};
   std::shared_ptr<PathWithLaneId> prev_module_reference_path_{};

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
@@ -16,6 +16,7 @@
 #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__INTERFACE_HPP_
 
 #include "behavior_path_planner/marker_util/lane_change/debug.hpp"
+#include "behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp"
 #include "behavior_path_planner/scene_module/lane_change/base_class.hpp"
 #include "behavior_path_planner/scene_module/lane_change/external_request.hpp"
 #include "behavior_path_planner/scene_module/lane_change/normal.hpp"
@@ -74,6 +75,8 @@ class LaneChangeInterface : public SceneModuleInterface
 
   ModuleStatus updateState() override;
 
+  void updateData() override;
+
   BehaviorModuleOutput plan() override;
 
   BehaviorModuleOutput planWaitingApproval() override;
@@ -113,6 +116,22 @@ class LaneChangeInterface : public SceneModuleInterface
   bool is_abort_approval_requested_{false};
 };
 
+class AvoidanceByLaneChangeInterface : public LaneChangeInterface
+{
+public:
+  AvoidanceByLaneChangeInterface(
+    const std::string & name, rclcpp::Node & node,
+    const std::shared_ptr<LaneChangeParameters> & parameters,
+    const std::shared_ptr<AvoidanceParameters> & avoidance_parameters,
+    const std::shared_ptr<AvoidanceByLCParameters> & avoidance_by_lane_change_parameters,
+    const std::unordered_map<std::string, std::shared_ptr<RTCInterface> > & rtc_interface_ptr_map);
+
+  ModuleStatus updateState() override;
+
+protected:
+  void updateRTCStatus(const double start_distance, const double finish_distance) override;
+};
+
 class LaneChangeBTInterface : public LaneChangeInterface
 {
 public:
@@ -150,16 +169,7 @@ class LaneChangeBTModule : public LaneChangeBTInterface
     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());
-  }
+  void updateRTCStatus(const double start_distance, const double finish_distance) override;
 };
 
 class ExternalRequestLaneChangeLeftBTModule : public LaneChangeBTInterface

diff --git a/.../behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp b/.../behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp
@@ -42,7 +42,7 @@ class LaneChangeModuleManager : public SceneModuleManagerInterface
 
   void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;
 
-private:
+protected:
   std::shared_ptr<LaneChangeParameters> parameters_;
 
   std::vector<std::shared_ptr<LaneChangeInterface>> registered_modules_;
@@ -52,6 +52,23 @@ class LaneChangeModuleManager : public SceneModuleManagerInterface
   LaneChangeModuleType type_;
 };
 
+class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager
+{
+public:
+  AvoidanceByLaneChangeModuleManager(
+    rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config,
+    std::shared_ptr<LaneChangeParameters> parameters,
+    std::shared_ptr<AvoidanceParameters> avoidance_parameters,
+    std::shared_ptr<AvoidanceByLCParameters> avoidance_by_lane_change_parameters);
+
+  std::shared_ptr<SceneModuleInterface> createNewSceneModuleInstance() override;
+
+  // void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;
+
+private:
+  std::shared_ptr<AvoidanceParameters> avoidance_parameters_;
+  std::shared_ptr<AvoidanceByLCParameters> avoidance_by_lane_change_parameters_;
+};
 }  // namespace behavior_path_planner
 
 #endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__MANAGER_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
@@ -68,7 +68,8 @@ class NormalLaneChange : public LaneChangeBase
 protected:
   lanelet::ConstLanelets getCurrentLanes() const override;
 
-  lanelet::ConstLanelets getLaneChangeLanes(const lanelet::ConstLanelets & current_lanes) const;
+  lanelet::ConstLanelets getLaneChangeLanes(
+    const lanelet::ConstLanelets & current_lanes, Direction direction) const override;
 
   int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const override;
 
@@ -79,7 +80,7 @@ class NormalLaneChange : public LaneChangeBase
 
   bool getLaneChangePaths(
     const lanelet::ConstLanelets & original_lanelets,
-    const lanelet::ConstLanelets & target_lanelets,
+    const lanelet::ConstLanelets & target_lanelets, Direction direction,
     LaneChangePaths * candidate_paths) const override;
 
   std::vector<DrivableLanes> getDrivableLanes() const override;

diff --git a/...behavior_path_planner/include/behavior_path_planner/utils/avoidance_by_lc/module_data.hpp b/...behavior_path_planner/include/behavior_path_planner/utils/avoidance_by_lc/module_data.hpp
diff --git a/..._path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/..._path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp
@@ -14,10 +14,12 @@
 #ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_
 #define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_
 
+#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp"
 #include "lanelet2_core/geometry/Lanelet.h"
 
 #include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp"
 
+#include <memory>
 #include <string>
 #include <vector>
 
@@ -79,7 +81,26 @@ struct LaneChangeTargetObjectIndices
   std::vector<size_t> other_lane{};
 };
 
-enum class LaneChangeModuleType { NORMAL = 0, EXTERNAL_REQUEST };
+enum class LaneChangeModuleType {
+  NORMAL = 0,
+  EXTERNAL_REQUEST,
+  AVOIDANCE_BY_LANE_CHANGE,
+};
+
+struct AvoidanceByLCParameters
+{
+  std::shared_ptr<AvoidanceParameters> avoidance{};
+  std::shared_ptr<LaneChangeParameters> lane_change{};
+
+  // execute if the target object number is larger than this param.
+  size_t execute_object_num{1};
+
+  // execute only when the target object longitudinal distance is larger than this param.
+  double execute_object_longitudinal_margin{0.0};
+
+  // execute only when lane change end point is before the object.
+  bool execute_only_when_lane_change_finish_before_object{false};
+};
 }  // namespace behavior_path_planner
 
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp
@@ -79,22 +79,6 @@ std::optional<LaneChangePath> constructCandidatePath(
   const BehaviorPathPlannerParameters & common_parameter,
   const LaneChangeParameters & lane_change_param);
 
-bool getLaneChangePaths(
-  const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets,
-  const lanelet::ConstLanelets & target_lanelets, const Pose & pose, const Twist & twist,
-  const PredictedObjects::ConstSharedPtr dynamic_objects,
-  const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & parameter,
-  const double check_length, LaneChangePaths * candidate_paths,
-  std::unordered_map<std::string, CollisionCheckDebug> * debug_data);
-
-bool getLaneChangePaths(
-  const PathWithLaneId & original_path, const RouteHandler & route_handler,
-  const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets,
-  const Pose & pose, const Twist & twist, const PredictedObjects::ConstSharedPtr dynamic_objects,
-  const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & parameter,
-  const double check_length, const Direction direction, LaneChangePaths * candidate_paths,
-  std::unordered_map<std::string, CollisionCheckDebug> * debug_data);
-
 bool isLaneChangePathSafe(
   const LaneChangePath & lane_change_path, const PredictedObjects::ConstSharedPtr dynamic_objects,
   const LaneChangeTargetObjectIndices & dynamic_object_indices, const Pose & current_pose,