Merge pull request #1223 from tier4/cp-lane-change-intersection

fix(lane_change): cherry pick for improvements

Author: shmpwk

planning/behavior_path_lane_change_module/README.md

@@ -89,7 +89,9 @@
         stop_time: 3.0 # [s]
 
       # collision check
-      enable_prepare_segment_collision_check: true
+      enable_collision_check_for_prepare_phase:
+        general_lanes: false
+        intersection: 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/config/lane_change.param.yaml

@@ -172,6 +172,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/images/lane_change-lane_change_phases.png

@@ -107,7 +107,8 @@ 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};
   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/images/lane_change-lane_expansion-with.png

@@ -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>
 
@@ -189,7 +191,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);
@@ -224,5 +226,57 @@ 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 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/images/lane_change-lane_expansion-without.png

@@ -68,8 +68,10 @@ void LaneChangeModuleManager::initParams(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.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/include/behavior_path_lane_change_module/scene.hpp

@@ -870,23 +870,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);
   }
 
@@ -950,8 +954,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);
 
@@ -1270,9 +1274,8 @@ bool NormalLaneChange::getLaneChangePaths(
         (prepare_duration < 1e-3) ? 0.0
                                   : ((prepare_velocity - current_velocity) / prepare_duration);
 
-      const double prepare_length =
-        current_velocity * prepare_duration +
-        0.5 * longitudinal_acc_on_prepare * std::pow(prepare_duration, 2);
+      const auto prepare_length = utils::lane_change::calcPhaseLength(
+        current_velocity, getCommonParam().max_vel, longitudinal_acc_on_prepare, prepare_duration);
 
       auto prepare_segment = getPrepareSegment(current_lanes, backward_path_length, prepare_length);
 
@@ -1324,9 +1327,9 @@ bool NormalLaneChange::getLaneChangePaths(
           utils::lane_change::calcLaneChangingAcceleration(
             initial_lane_changing_velocity, max_path_velocity, lane_changing_time,
             sampled_longitudinal_acc);
-        const auto lane_changing_length =
-          initial_lane_changing_velocity * lane_changing_time +
-          0.5 * longitudinal_acc_on_lane_changing * lane_changing_time * lane_changing_time;
+        const auto lane_changing_length = utils::lane_change::calcPhaseLength(
+          initial_lane_changing_velocity, getCommonParam().max_vel,
+          longitudinal_acc_on_lane_changing, lane_changing_time);
         const auto terminal_lane_changing_velocity = std::min(
           initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time,
           getCommonParam().max_vel);
@@ -2118,4 +2121,33 @@ 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);
+  });
+
+  return check_in_intersection || check_in_general_lanes;
+}
+
 }  // namespace behavior_path_planner

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

@@ -31,7 +31,11 @@
 #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 <lanelet2_core/LaneletMap.h>
 #include <lanelet2_core/geometry/LineString.h>
@@ -435,7 +439,8 @@ PathWithLaneId getReferencePathFromTargetLane(
       .get_child("getReferencePathFromTargetLane"),
     "start: %f, end: %f", s_start, s_end);
 
-  if (s_end - s_start < lane_changing_length) {
+  constexpr double epsilon = 1e-4;
+  if (s_end - s_start + epsilon < lane_changing_length) {
     return PathWithLaneId();
   }
 
@@ -1123,7 +1128,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;
@@ -1133,8 +1138,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;
@@ -1192,4 +1195,40 @@ 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())));
+}
+
+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