Merge pull request #1226 from zulfaqar-azmi-t4/cp-lane-change-interse…

…ction-turns

fix(lane_change): solve lane changing after intersection and/or turning

planning/behavior_path_lane_change_module/README.md

@@ -409,6 +409,16 @@ The following parameters are configurable in `behavior_path_planner.param.yaml`
 | `safety_check.stuck.check_objects_on_other_lanes`             | [-]     | boolean | If true, the lane change module include objects on other lanes.                                                                                                | true          |
 | `safety_check.stuck.use_all_predicted_path`                   | [-]     | boolean | If false, use only the predicted path that has the maximum confidence.                                                                                         | true          |
 
+| 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.                                                       | 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         |
+| `use_all_predicted_path`                                 | [-]   | boolean | If false, use only the predicted path that has the maximum confidence.                                                                                                                                     | true          |
+
 (\*1) the value must be negative.
 
 ### Abort lane change

planning/behavior_path_lane_change_module/config/lane_change.param.yaml

@@ -89,7 +89,10 @@
         stop_time: 3.0 # [s]
 
       # collision check
-      enable_prepare_segment_collision_check: true
+      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

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp

@@ -166,6 +166,8 @@ class NormalLaneChange : public LaneChangeBase
 
   bool isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const;
 
+  bool check_prepare_phase() const;
+
   double calcMaximumLaneChangeLength(
     const lanelet::ConstLanelet & current_terminal_lanelet, const double max_acc) const;
 

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp

@@ -107,7 +107,9 @@ struct LaneChangeParameters
   double max_longitudinal_acc{1.0};
 
   // collision check
-  bool enable_prepare_segment_collision_check{true};
+  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};

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp

@@ -23,6 +23,7 @@
 #include "rclcpp/logger.hpp"
 
 #include <route_handler/route_handler.hpp>
+#include <tier4_autoware_utils/geometry/boost_geometry.hpp>
 
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
@@ -31,6 +32,7 @@
 
 #include <lanelet2_core/Forward.h>
 
+#include <memory>
 #include <string>
 #include <vector>
 
@@ -185,7 +187,7 @@ std::optional<lanelet::BasicPolygon2d> createPolygon(
 
 ExtendedPredictedObject transform(
   const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters,
-  const LaneChangeParameters & lane_change_parameters);
+  const LaneChangeParameters & lane_change_parameters, const bool check_at_prepare_phase);
 
 bool isCollidedPolygonsInLanelet(
   const std::vector<Polygon2d> & collided_polygons, const lanelet::ConstLanelets & lanes);
@@ -220,5 +222,72 @@ lanelet::ConstLanelets generateExpandedLanelets(
  * @return rclcpp::Logger The logger instance configured for the specified lane change type.
  */
 rclcpp::Logger getLogger(const std::string & type);
+
+/**
+ * @brief Computes the current footprint of the ego vehicle based on its pose and size.
+ *
+ * This function calculates the 2D polygon representing the current footprint of the ego vehicle.
+ * The footprint is determined by the vehicle's pose and its dimensions, including the distance
+ * from the base to the front and rear ends of the vehicle, as well as its width.
+ *
+ * @param ego_pose The current pose of the ego vehicle.
+ * @param ego_info The structural information of the ego vehicle, such as its maximum longitudinal
+ *                 offset, rear overhang, and width.
+ *
+ * @return Polygon2d A polygon representing the current 2D footprint of the ego vehicle.
+ */
+Polygon2d getEgoCurrentFootprint(
+  const Pose & ego_pose, const vehicle_info_util::VehicleInfo & ego_info);
+
+/**
+ * @brief Checks if the given polygon is within an intersection area.
+ *
+ * This function evaluates whether a specified polygon is located within the bounds of an
+ * intersection. It identifies the intersection area by checking the attributes of the provided
+ * lanelet. If the lanelet has an attribute indicating it is part of an intersection, the function
+ * then checks if the polygon is fully contained within this area.
+ *
+ * @param route_handler a shared pointer to the route_handler
+ * @param lanelet A lanelet to check against the
+ *                intersection area.
+ * @param polygon The polygon to check for containment within the intersection area.
+ *
+ * @return bool True if the polygon is within the intersection area, false otherwise.
+ */
+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_

planning/behavior_path_lane_change_module/src/manager.cpp

@@ -64,8 +64,12 @@ void LaneChangeModuleManager::init(rclcpp::Node * node)
   p.max_longitudinal_acc = getOrDeclareParameter<double>(*node, parameter("max_longitudinal_acc"));
 
   // collision check
-  p.enable_prepare_segment_collision_check =
-    getOrDeclareParameter<bool>(*node, parameter("enable_prepare_segment_collision_check"));
+  p.enable_collision_check_for_prepare_phase_in_general_lanes = getOrDeclareParameter<bool>(
+    *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 =

planning/behavior_path_lane_change_module/src/scene.cpp

@@ -802,23 +802,27 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects(
   target_objects.other_lane.reserve(target_obj_index.other_lane.size());
 
   // objects in current lane
+  const auto is_check_prepare_phase = check_prepare_phase();
   for (const auto & obj_idx : target_obj_index.current_lane) {
     const auto extended_object = utils::lane_change::transform(
-      objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_);
+      objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_,
+      is_check_prepare_phase);
     target_objects.current_lane.push_back(extended_object);
   }
 
   // objects in target lane
   for (const auto & obj_idx : target_obj_index.target_lane) {
     const auto extended_object = utils::lane_change::transform(
-      objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_);
+      objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_,
+      is_check_prepare_phase);
     target_objects.target_lane.push_back(extended_object);
   }
 
   // objects in other lane
   for (const auto & obj_idx : target_obj_index.other_lane) {
     const auto extended_object = utils::lane_change::transform(
-      objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_);
+      objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_,
+      is_check_prepare_phase);
     target_objects.other_lane.push_back(extended_object);
   }
 
@@ -878,8 +882,8 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
     const auto obj_velocity_norm = std::hypot(
       object.kinematics.initial_twist_with_covariance.twist.linear.x,
       object.kinematics.initial_twist_with_covariance.twist.linear.y);
-    const auto extended_object =
-      utils::lane_change::transform(object, common_parameters, *lane_change_parameters_);
+    const auto extended_object = utils::lane_change::transform(
+      object, common_parameters, *lane_change_parameters_, check_prepare_phase());
 
     const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object);
 
@@ -1878,4 +1882,41 @@ void NormalLaneChange::updateStopTime()
   stop_watch_.tic("stop_time");
 }
 
+bool NormalLaneChange::check_prepare_phase() const
+{
+  const auto & route_handler = getRouteHandler();
+  const auto & vehicle_info = getCommonParam().vehicle_info;
+
+  const auto check_in_general_lanes =
+    lane_change_parameters_->enable_collision_check_for_prepare_phase_in_general_lanes;
+
+  lanelet::ConstLanelet current_lane;
+  if (!route_handler->getClosestLaneletWithinRoute(getEgoPose(), &current_lane)) {
+    RCLCPP_DEBUG(
+      logger_, "Unable to get current lane. Default to %s.",
+      (check_in_general_lanes ? "true" : "false"));
+    return check_in_general_lanes;
+  }
+
+  const auto ego_footprint = utils::lane_change::getEgoCurrentFootprint(getEgoPose(), vehicle_info);
+
+  const auto check_in_intersection = std::invoke([&]() {
+    if (!lane_change_parameters_->enable_collision_check_for_prepare_phase_in_intersection) {
+      return false;
+    }
+
+    return utils::lane_change::isWithinIntersection(route_handler, current_lane, ego_footprint);
+  });
+
+  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

planning/behavior_path_lane_change_module/src/utils/utils.cpp

@@ -30,7 +30,13 @@
 #include <motion_utils/trajectory/path_with_lane_id.hpp>
 #include <motion_utils/trajectory/trajectory.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/geometry/boost_geometry.hpp>
 #include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
+#include <vehicle_info_util/vehicle_info.hpp>
+
+#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>
@@ -1099,7 +1105,7 @@ std::optional<lanelet::BasicPolygon2d> createPolygon(
 ExtendedPredictedObject transform(
   const PredictedObject & object,
   [[maybe_unused]] const BehaviorPathPlannerParameters & common_parameters,
-  const LaneChangeParameters & lane_change_parameters)
+  const LaneChangeParameters & lane_change_parameters, const bool check_at_prepare_phase)
 {
   ExtendedPredictedObject extended_object;
   extended_object.uuid = object.object_id;
@@ -1109,8 +1115,6 @@ ExtendedPredictedObject transform(
   extended_object.shape = object.shape;
 
   const auto & time_resolution = lane_change_parameters.prediction_time_resolution;
-  const auto & check_at_prepare_phase =
-    lane_change_parameters.enable_prepare_segment_collision_check;
   const auto & prepare_duration = lane_change_parameters.lane_change_prepare_duration;
   const auto & velocity_threshold =
     lane_change_parameters.prepare_segment_ignore_object_velocity_thresh;
@@ -1168,4 +1172,51 @@ rclcpp::Logger getLogger(const std::string & type)
 {
   return rclcpp::get_logger("lane_change").get_child(type);
 }
+
+Polygon2d getEgoCurrentFootprint(
+  const Pose & ego_pose, const vehicle_info_util::VehicleInfo & ego_info)
+{
+  const auto base_to_front = ego_info.max_longitudinal_offset_m;
+  const auto base_to_rear = ego_info.rear_overhang_m;
+  const auto width = ego_info.vehicle_width_m;
+
+  return tier4_autoware_utils::toFootprint(ego_pose, base_to_front, base_to_rear, width);
+}
+
+bool isWithinIntersection(
+  const std::shared_ptr<RouteHandler> & route_handler, const lanelet::ConstLanelet & lanelet,
+  const Polygon2d & polygon)
+{
+  const std::string id = lanelet.attributeOr("intersection_area", "else");
+  if (id == "else") {
+    return false;
+  }
+
+  const auto lanelet_polygon =
+    route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(id.c_str()));
+
+  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