feat(lane_change): check prepare phase in turn direction lanes (autow…

…arefoundation#6726) * feat(lane_change): check prepare phase in turn direction lanes Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]> * Doxygen comment Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]> * Add config Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]> * fix comments by the reviewer Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]> --------- Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>
tier4 · Apr 10, 2024 · 712ab11 · 712ab11
1 parent d0c6071
commit 712ab11
Show file tree

Hide file tree

Showing 7 changed files with 43 additions and 2 deletions.
diff --git a/planning/behavior_path_lane_change_module/README.md b/planning/behavior_path_lane_change_module/README.md
@@ -524,7 +524,8 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi
 | Name                                                     | Unit  | Type    | Description                                                                                                                                                                                                | Default value |
 | :------------------------------------------------------- | ----- | ------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- |
 | `enable_collision_check_for_prepare_phase.general_lanes` | [-]   | boolean | Perform collision check starting from the prepare phase for situations not explicitly covered by other settings (e.g., intersections). If `false`, collision check only evaluated for lane changing phase. | false         |
-| `enable_collision_check_for_prepare_phase.intersection`  | [-]   | boolean | Perform collision check starting from prepare phase when ego is in intersection. If `false`, collision check only evaluated for lane changing phase.                                                       | false         |
+| `enable_collision_check_for_prepare_phase.intersection`  | [-]   | boolean | Perform collision check starting from prepare phase when ego is in intersection. If `false`, collision check only evaluated for lane changing phase.                                                       | true          |
+| `enable_collision_check_for_prepare_phase.turns`         | [-]   | boolean | Perform collision check starting from prepare phase when ego is in lanelet with turn direction tags. If `false`, collision check only evaluated for lane changing phase.                                   | true          |
 | `prepare_phase_ignore_target_speed_thresh`               | [m/s] | double  | Ignore collision check in prepare phase of object speed that is lesser that the configured value. `enable_collision_check_at_prepare_phase` must be `true`                                                 | 0.1           |
 | `check_objects_on_current_lanes`                         | [-]   | boolean | If true, the lane change module check objects on current lanes when performing collision assessment.                                                                                                       | false         |
 | `check_objects_on_other_lanes`                           | [-]   | boolean | If true, the lane change module include objects on other lanes. when performing collision assessment                                                                                                       | false         |

diff --git a/planning/behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_lane_change_module/config/lane_change.param.yaml
@@ -92,6 +92,7 @@
       enable_collision_check_for_prepare_phase:
         general_lanes: false
         intersection: true
+        turns: true
       prepare_segment_ignore_object_velocity_thresh: 0.1 # [m/s]
       check_objects_on_current_lanes: true
       check_objects_on_other_lanes: true

diff --git a/...r_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp b/...r_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp
@@ -109,6 +109,7 @@ struct LaneChangeParameters
   // collision check
   bool enable_collision_check_for_prepare_phase_in_general_lanes{false};
   bool enable_collision_check_for_prepare_phase_in_intersection{true};
+  bool enable_collision_check_for_prepare_phase_in_turns{true};
   double prepare_segment_ignore_object_velocity_thresh{0.1};
   bool check_objects_on_current_lanes{true};
   bool check_objects_on_other_lanes{true};

diff --git a/...behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp b/...behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp
@@ -262,6 +262,21 @@ bool isWithinIntersection(
   const std::shared_ptr<RouteHandler> & route_handler, const lanelet::ConstLanelet & lanelet,
   const Polygon2d & polygon);
 
+/**
+ * @brief Determines if a polygon is within lanes designated for turning.
+ *
+ * Checks if a polygon overlaps with lanelets tagged for turning directions (excluding 'straight').
+ * It evaluates the lanelet's 'turn_direction' attribute and determines overlap with the lanelet's
+ * area.
+ *
+ * @param lanelet Lanelet representing the road segment whose turn direction is to be evaluated.
+ * @param polygon The polygon to be checked for its presence within turn direction lanes.
+ *
+ * @return bool True if the polygon is within a lane designated for turning, false if it is within a
+ *              straight lane or no turn direction is specified.
+ */
+bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon);
+
 /**
  * @brief Calculates the distance required during a lane change operation.
  *

diff --git a/planning/behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_lane_change_module/src/manager.cpp
@@ -72,6 +72,8 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node)
     *node, parameter("enable_collision_check_for_prepare_phase.general_lanes"));
   p.enable_collision_check_for_prepare_phase_in_intersection = getOrDeclareParameter<bool>(
     *node, parameter("enable_collision_check_for_prepare_phase.intersection"));
+  p.enable_collision_check_for_prepare_phase_in_turns =
+    getOrDeclareParameter<bool>(*node, parameter("enable_collision_check_for_prepare_phase.turns"));
   p.prepare_segment_ignore_object_velocity_thresh = getOrDeclareParameter<double>(
     *node, parameter("prepare_segment_ignore_object_velocity_thresh"));
   p.check_objects_on_current_lanes =

diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp
@@ -2147,7 +2147,15 @@ bool NormalLaneChange::check_prepare_phase() const
     return utils::lane_change::isWithinIntersection(route_handler, current_lane, ego_footprint);
   });
 
-  return check_in_intersection || check_in_general_lanes;
+  const auto check_in_turns = std::invoke([&]() {
+    if (!lane_change_parameters_->enable_collision_check_for_prepare_phase_in_turns) {
+      return false;
+    }
+
+    return utils::lane_change::isWithinTurnDirectionLanes(current_lane, ego_footprint);
+  });
+
+  return check_in_intersection || check_in_turns || check_in_general_lanes;
 }
 
 }  // namespace behavior_path_planner
diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp
@@ -37,6 +37,8 @@
 
 #include <geometry_msgs/msg/detail/pose__struct.hpp>
 
+#include <boost/geometry/algorithms/detail/disjoint/interface.hpp>
+
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_core/geometry/LineString.h>
 #include <lanelet2_core/geometry/Point.h>
@@ -1222,6 +1224,17 @@ bool isWithinIntersection(
     polygon, utils::toPolygon2d(lanelet::utils::to2D(lanelet_polygon.basicPolygon())));
 }
 
+bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon)
+{
+  const std::string turn_direction = lanelet.attributeOr("turn_direction", "else");
+  if (turn_direction == "else" || turn_direction == "straight") {
+    return false;
+  }
+
+  return !boost::geometry::disjoint(
+    polygon, utils::toPolygon2d(lanelet::utils::to2D(lanelet.polygon2d().basicPolygon())));
+}
+
 double calcPhaseLength(
   const double velocity, const double maximum_velocity, const double acceleration,
   const double duration)