fix(lane_change): enable cancel when ego in turn direction lane (auto…

…warefoundation#9124) * RT0-33893 add checks from prev intersection Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]> Signed-off-by: Zulfaqar Azmi <[email protected]> * fix shadow variable Signed-off-by: Zulfaqar Azmi <[email protected]> * fix logic Signed-off-by: Zulfaqar Azmi <[email protected]> * update readme Signed-off-by: Zulfaqar Azmi <[email protected]> * refactor get_ego_footprint Signed-off-by: Zulfaqar Azmi <[email protected]> --------- Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]> Signed-off-by: Zulfaqar Azmi <[email protected]>
tier4 · Nov 7, 2024 · 663fe98 · 663fe98
1 parent d50516f
commit 663fe98
Show file tree

Hide file tree

Showing 8 changed files with 220 additions and 33 deletions.
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md
@@ -823,6 +823,7 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi
 | `prepare_duration`                           | [m]    | double | The preparation time for the ego vehicle to be ready to perform lane change.                                           | 4.0                |
 | `backward_length_buffer_for_end_of_lane`     | [m]    | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change                                  | 3.0                |
 | `backward_length_buffer_for_blocking_object` | [m]    | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change when there is an object in front | 3.0                |
+| `backward_length_from_intersection`          | [m]    | double | Distance threshold from the last intersection to invalidate or cancel the lane change path                             | 5.0                |
 | `lane_change_finish_judge_buffer`            | [m]    | double | The additional buffer used to confirm lane change process completion                                                   | 2.0                |
 | `lane_changing_lateral_jerk`                 | [m/s3] | double | Lateral jerk value for lane change path generation                                                                     | 0.5                |
 | `minimum_lane_changing_velocity`             | [m/s]  | double | Minimum speed during lane changing process.                                                                            | 2.78               |

diff --git a/...vior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/...vior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml
@@ -6,6 +6,7 @@
 
       backward_length_buffer_for_end_of_lane: 3.0 # [m]
       backward_length_buffer_for_blocking_object: 3.0 # [m]
+      backward_length_from_intersection: 5.0 # [m]
 
       lane_changing_lateral_jerk: 0.5              # [m/s3]
 

diff --git a/...avior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/...avior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp
@@ -211,6 +211,9 @@ class NormalLaneChange : public LaneChangeBase
     return common_data_ptr_->lanes_ptr->target;
   }
 
+  void update_dist_from_intersection();
+
+  std::vector<PathPointWithLaneId> path_after_intersection_;
   double stop_time_{0.0};
   static constexpr double floating_err_th{1e-3};
 };

diff --git a/...ne_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/...ne_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp
@@ -39,6 +39,7 @@ using geometry_msgs::msg::Twist;
 using nav_msgs::msg::Odometry;
 using route_handler::Direction;
 using route_handler::RouteHandler;
+using universe_utils::Polygon2d;
 using utils::path_safety_checker::ExtendedPredictedObjects;
 
 struct LateralAccelerationMap
@@ -110,6 +111,7 @@ struct Parameters
   // lane change parameters
   double backward_length_buffer_for_end_of_lane{0.0};
   double backward_length_buffer_for_blocking_object{0.0};
+  double backward_length_from_intersection{5.0};
   double lane_changing_lateral_jerk{0.5};
   double minimum_lane_changing_velocity{5.6};
   double lane_change_prepare_duration{4.0};
@@ -220,6 +222,7 @@ struct Lanes
 {
   bool current_lane_in_goal_section{false};
   bool target_lane_in_goal_section{false};
+  lanelet::ConstLanelet ego_lane;
   lanelet::ConstLanelets current;
   lanelet::ConstLanelets target_neighbor;
   lanelet::ConstLanelets target;
@@ -323,6 +326,8 @@ struct MinMaxValue
 
 struct TransientData
 {
+  Polygon2d current_footprint;  // ego's polygon at current pose
+
   MinMaxValue lane_changing_length;  // lane changing length for a single lane change
   MinMaxValue
     current_dist_buffer;  // distance buffer computed backward from current lanes' terminal end
@@ -331,6 +336,8 @@ struct TransientData
   double dist_to_terminal_end{
     std::numeric_limits<double>::min()};  // distance from ego base link to the current lanes'
                                           // terminal end
+  double dist_from_prev_intersection{std::numeric_limits<double>::max()};
+  // terminal end
   double dist_to_terminal_start{
     std::numeric_limits<double>::min()};  // distance from ego base link to the current lanes'
                                           // terminal start
@@ -347,6 +354,9 @@ struct TransientData
 
   bool is_ego_near_current_terminal_start{false};
   bool is_ego_stuck{false};
+
+  bool in_turn_direction_lane{false};
+  bool in_intersection{false};
 };
 
 using RouteHandlerPtr = std::shared_ptr<RouteHandler>;

diff --git a/...path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/...path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp
@@ -44,6 +44,7 @@ using autoware::behavior_path_planner::utils::path_safety_checker::
 using autoware::behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped;
 using autoware::behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon;
 using autoware::route_handler::Direction;
+using autoware::universe_utils::LineString2d;
 using autoware::universe_utils::Polygon2d;
 using autoware::vehicle_info_utils::VehicleInfo;
 using autoware_perception_msgs::msg::PredictedObject;
@@ -188,13 +189,12 @@ rclcpp::Logger getLogger(const std::string & type);
  * 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.
+ * @param common_data_ptr Shared pointer to CommonData that holds necessary ego vehicle's dimensions
+ *                        and pose information.
  *
  * @return Polygon2d A polygon representing the current 2D footprint of the ego vehicle.
  */
-Polygon2d getEgoCurrentFootprint(const Pose & ego_pose, const VehicleInfo & ego_info);
+Polygon2d get_ego_footprint(const Pose & ego_pose, const VehicleInfo & ego_info);
 
 Point getEgoFrontVertex(const Pose & ego_pose, const VehicleInfo & ego_info, bool left);
 
@@ -213,7 +213,7 @@ Point getEgoFrontVertex(const Pose & ego_pose, const VehicleInfo & ego_info, boo
  *
  * @return bool True if the polygon is within the intersection area, false otherwise.
  */
-bool isWithinIntersection(
+bool is_within_intersection(
   const std::shared_ptr<RouteHandler> & route_handler, const lanelet::ConstLanelet & lanelet,
   const Polygon2d & polygon);
 
@@ -230,7 +230,8 @@ bool isWithinIntersection(
  * @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);
+bool is_within_turn_direction_lanes(
+  const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon);
 
 LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr);
 
@@ -297,6 +298,51 @@ double get_min_dist_to_current_lanes_obj(
 bool has_blocking_target_object(
   const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects,
   const double stop_arc_length, const PathWithLaneId & path);
-}  // namespace autoware::behavior_path_planner::utils::lane_change
 
+/**
+ * @brief Checks if the ego vehicle has passed any turn direction within an intersection.
+ *
+ * This function determines whether the ego vehicle has exited the intersection and
+ * turn lane area based on its distance from the previous intersection. It considers
+ * whether the ego vehicle is currently in an intersection and a turn lane.
+ *
+ * @param common_data_ptr Shared pointer to CommonData containing the transient data and
+ *                        lane-change parameters required for the distance's comparison.
+ *
+ * @return true if the ego vehicle has passed the intersection turn direction, false otherwise.
+ */
+bool has_passed_intersection_turn_direction(const CommonDataPtr & common_data_ptr);
+
+/**
+ * @brief Retrieves the predicted paths of an object as 2D line strings.
+ *
+ * This function transforms each predicted path of an object into a LineString2d, representing
+ * a 2D sequence of points. Each point in the path is extracted from the predicted path's
+ * position and converted to a 2D point.
+ *
+ * @param object The predicted object whose paths will be converted into 2D line strings.
+ *
+ * @return std::vector<LineString2d> A vector of 2D line strings representing the predicted paths
+ *                                   of the object.
+ */
+std::vector<LineString2d> get_line_string_paths(const ExtendedPredictedObject & object);
+
+/**
+ * @brief Determines if there is an object in the turn lane that could overtake the ego vehicle.
+ *
+ * This function checks for any trailing objects in the turn lane that may attempt to overtake
+ * the ego vehicle. The check is only applicable if the ego vehicle is still within a certain
+ * distance from the previous intersection's turn lane. It evaluates whether any of the predicted
+ * paths or the initial polygon of trailing objects overlap with the target lane polygon.
+ *
+ * @param common_data_ptr Shared pointer to CommonData containing lane and polygon information
+ *                        for the ego vehicle.
+ * @param trailing_objects A collection of predicted objects trailing the ego vehicle.
+ *
+ * @return true if there is an object in the turn lane with a potential to overtake, false
+ * otherwise.
+ */
+bool has_overtaking_turn_lane_object(
+  const CommonDataPtr & common_data_ptr, const ExtendedPredictedObjects & trailing_objects);
+}  // namespace autoware::behavior_path_planner::utils::lane_change
 #endif  // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp
@@ -177,6 +177,8 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s
     getOrDeclareParameter<double>(*node, parameter("backward_length_buffer_for_end_of_lane"));
   p.backward_length_buffer_for_blocking_object =
     getOrDeclareParameter<double>(*node, parameter("backward_length_buffer_for_blocking_object"));
+  p.backward_length_from_intersection =
+    getOrDeclareParameter<double>(*node, parameter("backward_length_from_intersection"));
   p.lane_changing_lateral_jerk =
     getOrDeclareParameter<double>(*node, parameter("lane_changing_lateral_jerk"));
   p.lane_change_prepare_duration =

diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp
@@ -75,6 +75,13 @@ void NormalLaneChange::update_lanes(const bool is_approved)
     return;
   }
 
+  lanelet::ConstLanelet current_lane;
+  if (!common_data_ptr_->route_handler_ptr->getClosestLaneletWithinRoute(
+        common_data_ptr_->get_ego_pose(), &current_lane)) {
+    return;
+  }
+
+  common_data_ptr_->lanes_ptr->ego_lane = current_lane;
   const auto is_same_lanes_with_prev_iteration =
     utils::lane_change::is_same_lane_with_prev_iteration(
       common_data_ptr_, current_lanes, target_lanes);
@@ -93,6 +100,8 @@ void NormalLaneChange::update_lanes(const bool is_approved)
   common_data_ptr_->lanes_ptr->target_neighbor = utils::lane_change::get_target_neighbor_lanes(
     *route_handler_ptr, current_lanes, common_data_ptr_->lc_type);
 
+  common_data_ptr_->current_lanes_path =
+    route_handler_ptr->getCenterLinePath(current_lanes, 0.0, std::numeric_limits<double>::max());
   common_data_ptr_->lanes_ptr->current_lane_in_goal_section =
     route_handler_ptr->isInGoalRouteSection(current_lanes.back());
   common_data_ptr_->lanes_ptr->target_lane_in_goal_section =
@@ -150,6 +159,18 @@ void NormalLaneChange::update_transient_data()
   transient_data.is_ego_near_current_terminal_start =
     transient_data.dist_to_terminal_start < transient_data.max_prepare_length;
 
+  transient_data.current_footprint = utils::lane_change::get_ego_footprint(
+    common_data_ptr_->get_ego_pose(), common_data_ptr_->bpp_param_ptr->vehicle_info);
+
+  const auto & ego_lane = common_data_ptr_->lanes_ptr->ego_lane;
+  const auto & route_handler_ptr = common_data_ptr_->route_handler_ptr;
+  transient_data.in_intersection = utils::lane_change::is_within_intersection(
+    route_handler_ptr, ego_lane, transient_data.current_footprint);
+  transient_data.in_turn_direction_lane =
+    utils::lane_change::is_within_turn_direction_lanes(ego_lane, transient_data.current_footprint);
+
+  update_dist_from_intersection();
+
   updateStopTime();
   transient_data.is_ego_stuck = is_ego_stuck();
 
@@ -754,6 +775,10 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const
     return false;
   }
 
+  if (common_data_ptr_->transient_data.in_turn_direction_lane) {
+    return true;
+  }
+
   const auto nearest_idx = autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
     status_.lane_change_path.path.points, getEgoPose(),
     planner_data_->parameters.ego_nearest_dist_threshold,
@@ -1404,6 +1429,11 @@ bool NormalLaneChange::check_candidate_path_safety(
   const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const
 {
   const auto is_stuck = common_data_ptr_->transient_data.is_ego_stuck;
+  if (utils::lane_change::has_overtaking_turn_lane_object(
+        common_data_ptr_, filtered_objects_.target_lane_trailing)) {
+    throw std::logic_error("Ego is nearby intersection, and there might be overtaking vehicle.");
+  }
+
   if (
     !is_stuck && !utils::lane_change::passed_parked_objects(
                    common_data_ptr_, candidate_path, filtered_objects_.target_lane_leading,
@@ -1579,6 +1609,13 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const
 
   CollisionCheckDebugMap debug_data;
 
+  const auto has_overtaking_object = utils::lane_change::has_overtaking_turn_lane_object(
+    common_data_ptr_, filtered_objects_.target_lane_trailing);
+
+  if (has_overtaking_object) {
+    return {false, true};
+  }
+
   const auto has_passed_parked_objects = utils::lane_change::passed_parked_objects(
     common_data_ptr_, path, filtered_objects_.target_lane_leading, debug_data);
 
@@ -2076,7 +2113,6 @@ void NormalLaneChange::updateStopTime()
 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;
@@ -2089,24 +2125,52 @@ bool NormalLaneChange::check_prepare_phase() const
     return check_in_general_lanes;
   }
 
-  const auto ego_footprint = utils::lane_change::getEgoCurrentFootprint(getEgoPose(), vehicle_info);
+  const auto check_in_intersection =
+    lane_change_parameters_->enable_collision_check_for_prepare_phase_in_intersection &&
+    common_data_ptr_->transient_data.in_intersection;
 
-  const auto check_in_intersection = std::invoke([&]() {
-    if (!lane_change_parameters_->enable_collision_check_for_prepare_phase_in_intersection) {
-      return false;
-    }
+  const auto check_in_turns =
+    lane_change_parameters_->enable_collision_check_for_prepare_phase_in_turns &&
+    common_data_ptr_->transient_data.in_turn_direction_lane;
 
-    return utils::lane_change::isWithinIntersection(route_handler, current_lane, ego_footprint);
-  });
+  return check_in_intersection || check_in_turns || 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;
-    }
+void NormalLaneChange::update_dist_from_intersection()
+{
+  auto & transient_data = common_data_ptr_->transient_data;
+  const auto & route_handler_ptr = common_data_ptr_->route_handler_ptr;
 
-    return utils::lane_change::isWithinTurnDirectionLanes(current_lane, ego_footprint);
-  });
+  if (
+    transient_data.in_intersection && transient_data.in_turn_direction_lane &&
+    path_after_intersection_.empty()) {
+    auto path_after_intersection = route_handler_ptr->getCenterLinePath(
+      common_data_ptr_->lanes_ptr->target_neighbor, 0.0, std::numeric_limits<double>::max());
+    path_after_intersection_ = std::move(path_after_intersection.points);
+    transient_data.dist_from_prev_intersection = 0.0;
+    return;
+  }
 
-  return check_in_intersection || check_in_turns || check_in_general_lanes;
+  if (
+    transient_data.in_intersection || transient_data.in_turn_direction_lane ||
+    path_after_intersection_.empty()) {
+    return;
+  }
+
+  const auto & path_points = path_after_intersection_;
+  const auto & front_point = path_points.front().point.pose.position;
+  const auto & ego_position = common_data_ptr_->get_ego_pose().position;
+  transient_data.dist_from_prev_intersection =
+    calcSignedArcLength(path_points, front_point, ego_position);
+
+  if (
+    transient_data.dist_from_prev_intersection >= 0.0 &&
+    transient_data.dist_from_prev_intersection <=
+      common_data_ptr_->lc_param_ptr->backward_length_from_intersection) {
+    return;
+  }
+
+  path_after_intersection_.clear();
+  transient_data.dist_from_prev_intersection = std::numeric_limits<double>::max();
 }
 }  // namespace autoware::behavior_path_planner