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]>
zulfaqar-azmi-t4 · Apr 5, 2024 · 1b3e0fa · 1b3e0fa
1 parent 7c78e44
commit 1b3e0fa
Show file tree

Hide file tree

Showing 7 changed files with 70 additions and 2 deletions.
diff --git a/planning/behavior_path_lane_change_module/README.md b/planning/behavior_path_lane_change_module/README.md
@@ -412,7 +412,8 @@ The following parameters are configurable in `behavior_path_planner.param.yaml`
 | 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
@@ -257,5 +257,37 @@ Polygon2d getEgoCurrentFootprint(
 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.
+ *
+ * Used for computing prepare or lane change length based on current and maximum velocity,
+ * acceleration, and duration, returning the lesser of accelerated distance or distance at max
+ * velocity.
+ *
+ * @param velocity The current velocity of the vehicle in meters per second (m/s).
+ * @param maximum_velocity The maximum velocity the vehicle can reach in meters per second (m/s).
+ * @param acceleration The acceleration of the vehicle in meters per second squared (m/s^2).
+ * @param duration The duration of the lane change in seconds (s).
+ * @return The calculated minimum distance in meters (m).
+ */
+double calcPhaseLength(
+  const double velocity, const double maximum_velocity, const double acceleration,
+  const double time);
 }  // namespace behavior_path_planner::utils::lane_change
 #endif  // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_
diff --git a/planning/behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_lane_change_module/src/manager.cpp
@@ -68,6 +68,8 @@ void LaneChangeModuleManager::init(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
@@ -1908,7 +1908,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
@@ -36,6 +36,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>
@@ -1196,4 +1198,25 @@ bool isWithinIntersection(
   return boost::geometry::within(
     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)
+{
+  const auto length_with_acceleration =
+    velocity * duration + 0.5 * acceleration * std::pow(duration, 2);
+  const auto length_with_max_velocity = maximum_velocity * duration;
+  return std::min(length_with_acceleration, length_with_max_velocity);
+}
 }  // namespace behavior_path_planner::utils::lane_change