Merge pull request #897 from tier4/cp-lane-change-x2

fix(lane_change): cherry pick lane change PRs for x2

planning/behavior_path_planner/config/lane_change/lane_change.param.yaml

@@ -28,15 +28,28 @@
 
       # safety check
       safety_check:
-        expected_front_deceleration: -1.0
-        expected_rear_deceleration: -1.0
-        expected_front_deceleration_for_abort: -1.0
-        expected_rear_deceleration_for_abort: -2.0
-        rear_vehicle_reaction_time: 2.0
-        rear_vehicle_safety_time_margin: 1.0
-        lateral_distance_max_threshold: 2.0
-        longitudinal_distance_min_threshold: 3.0
-        longitudinal_velocity_delta_time: 0.8
+        allow_loose_check_for_cancel: true
+        execution:
+          expected_front_deceleration: -1.0
+          expected_rear_deceleration: -1.0
+          rear_vehicle_reaction_time: 2.0
+          rear_vehicle_safety_time_margin: 1.0
+          lateral_distance_max_threshold: 2.0
+          longitudinal_distance_min_threshold: 3.0
+          longitudinal_velocity_delta_time: 0.8
+        cancel:
+          expected_front_deceleration: -1.0
+          expected_rear_deceleration: -2.0
+          rear_vehicle_reaction_time: 1.5
+          rear_vehicle_safety_time_margin: 0.8
+          lateral_distance_max_threshold: 1.0
+          longitudinal_distance_min_threshold: 2.5
+          longitudinal_velocity_delta_time: 0.6
+
+        # lane expansion for object filtering
+        lane_expansion:
+          left_offset: 0.0 # [m]
+          right_offset: 0.0 # [m]
 
       # lateral acceleration map
       lateral_acceleration:

planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp

@@ -17,6 +17,7 @@
 
 #include "behavior_path_planner/marker_utils/utils.hpp"
 #include "behavior_path_planner/utils/lane_change/lane_change_path.hpp"
+#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp"
 
 #include <string>
 #include <unordered_map>
@@ -25,11 +26,16 @@
 namespace marker_utils::lane_change_markers
 {
 using behavior_path_planner::LaneChangePath;
+using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects;
 using visualization_msgs::msg::MarkerArray;
 MarkerArray showAllValidLaneChangePath(
   const std::vector<LaneChangePath> & lanes, std::string && ns);
 MarkerArray createLaneChangingVirtualWallMarker(
   const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name,
   const rclcpp::Time & now, const std::string & ns);
+MarkerArray showFilteredObjects(
+  const ExtendedPredictedObjects & current_lane_objects,
+  const ExtendedPredictedObjects & target_lane_objects,
+  const ExtendedPredictedObjects & other_lane_objects, const std::string & ns);
 }  // namespace marker_utils::lane_change_markers
 #endif  // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__LANE_CHANGE__DEBUG_HPP_

planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp

@@ -29,6 +29,7 @@
 
 #include <lanelet2_core/geometry/Lanelet.h>
 
+#include <cstdint>
 #include <string>
 #include <vector>
 
@@ -42,6 +43,7 @@ using behavior_path_planner::ShiftLineArray;
 using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap;
 using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugPair;
 using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject;
+using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Polygon;
 using geometry_msgs::msg::Pose;
@@ -109,6 +111,10 @@ MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::strin
 MarkerArray showPredictedPath(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
 
 MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
+
+MarkerArray showFilteredObjects(
+  const ExtendedPredictedObjects & predicted_objects, const std::string & ns,
+  const ColorRGBA & color, int32_t id);
 }  // namespace marker_utils
 
 #endif  // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__UTILS_HPP_

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp

@@ -154,6 +154,11 @@ class LaneChangeBase
     return object_debug_after_approval_;
   }
 
+  const LaneChangeTargetObjects & getDebugFilteredObjects() const
+  {
+    return debug_filtered_objects_;
+  }
+
   const Pose & getEgoPose() const { return planner_data_->self_odometry->pose.pose; }
 
   const Point & getEgoPosition() const { return getEgoPose().position; }
@@ -262,6 +267,7 @@ class LaneChangeBase
   mutable LaneChangePaths debug_valid_path_{};
   mutable CollisionCheckDebugMap object_debug_{};
   mutable CollisionCheckDebugMap object_debug_after_approval_{};
+  mutable LaneChangeTargetObjects debug_filtered_objects_{};
   mutable double object_debug_lifetime_{0.0};
   mutable StopWatch<std::chrono::milliseconds> stop_watch_;
 };

planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp

@@ -65,6 +65,8 @@ struct LaneChangeParameters
   bool check_objects_on_current_lanes{true};
   bool check_objects_on_other_lanes{true};
   bool use_all_predicted_path{false};
+  double lane_expansion_left_offset{0.0};
+  double lane_expansion_right_offset{0.0};
 
   // regulatory elements
   bool regulate_on_crosswalk{false};
@@ -78,8 +80,9 @@ struct LaneChangeParameters
   utils::path_safety_checker::ObjectTypesToCheck object_types_to_check;
 
   // safety check
-  utils::path_safety_checker::RSSparams rss_params;
-  utils::path_safety_checker::RSSparams rss_params_for_abort;
+  bool allow_loose_check_for_cancel{true};
+  utils::path_safety_checker::RSSparams rss_params{};
+  utils::path_safety_checker::RSSparams rss_params_for_abort{};
 
   // abort
   LaneChangeCancelParameters cancel;

planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp

@@ -188,5 +188,27 @@ std::optional<lanelet::BasicPolygon2d> createPolygon(
 ExtendedPredictedObject transform(
   const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters,
   const LaneChangeParameters & lane_change_parameters);
+
+bool isCollidedPolygonsInLanelet(
+  const std::vector<Polygon2d> & collided_polygons, const lanelet::ConstLanelets & lanes);
+
+/**
+ * @brief Generates expanded lanelets based on the given direction and offsets.
+ *
+ * Expands the provided lanelets in either the left or right direction based on
+ * the specified direction. If the direction is 'LEFT', the lanelets are expanded
+ * using the left_offset; if 'RIGHT', they are expanded using the right_offset.
+ * Otherwise, no expansion occurs.
+ *
+ * @param lanes The lanelets to be expanded.
+ * @param direction The direction of expansion: either LEFT or RIGHT.
+ * @param left_offset The offset value for left expansion.
+ * @param right_offset The offset value for right expansion.
+ * @return lanelet::ConstLanelets A collection of expanded lanelets.
+ */
+lanelet::ConstLanelets generateExpandedLanelets(
+  const lanelet::ConstLanelets & lanes, const Direction direction, const double left_offset,
+  const double right_offset);
+
 }  // namespace behavior_path_planner::utils::lane_change
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_

planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp

@@ -77,6 +77,7 @@ struct ExtendedPredictedObject
   autoware_auto_perception_msgs::msg::Shape shape;
   std::vector<PredictedPathWithPolygon> predicted_paths;
 };
+using ExtendedPredictedObjects = std::vector<ExtendedPredictedObject>;
 
 /**
  * @brief Specifies which object class should be checked.

planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp

@@ -83,7 +83,6 @@ boost::optional<PoseWithVelocityStamped> calcInterpolatedPoseWithVelocity(
 boost::optional<PoseWithVelocityAndPolygonStamped> getInterpolatedPoseWithVelocityAndPolygonStamped(
   const std::vector<PoseWithVelocityStamped> & pred_path, const double current_time,
   const VehicleInfo & ego_info);
-
 /**
  * @brief Iterate the points in the ego and target's predicted path and
  *        perform safety check for each of the iterated points.
@@ -106,6 +105,28 @@ bool checkCollision(
   const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters,
   const double hysteresis_factor, CollisionCheckDebug & debug);
 
+/**
+ * @brief Iterate the points in the ego and target's predicted path and
+ *        perform safety check for each of the iterated points.
+ * @param planned_path The predicted path of the ego vehicle.
+ * @param predicted_ego_path Ego vehicle's predicted path
+ * @param ego_current_velocity Current velocity of the ego vehicle.
+ * @param target_object The predicted object to check collision with.
+ * @param target_object_path The predicted path of the target object.
+ * @param common_parameters The common parameters used in behavior path planner.
+ * @param front_object_deceleration The deceleration of the object in the front.(used in RSS)
+ * @param rear_object_deceleration The deceleration of the object in the rear.(used in RSS)
+ * @param debug The debug information for collision checking.
+ * @return a list of polygon when collision is expected.
+ */
+std::vector<Polygon2d> getCollidedPolygons(
+  const PathWithLaneId & planned_path,
+  const std::vector<PoseWithVelocityStamped> & predicted_ego_path,
+  const ExtendedPredictedObject & target_object,
+  const PredictedPathWithPolygon & target_object_path,
+  const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters,
+  const double hysteresis_factor, CollisionCheckDebug & debug);
+
 /**
  * @brief Check collision between ego path footprints with extra longitudinal stopping margin and
  * objects.

planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp

@@ -98,4 +98,28 @@ MarkerArray createLaneChangingVirtualWallMarker(
   return marker_array;
 }
 
+MarkerArray showFilteredObjects(
+  const ExtendedPredictedObjects & current_lane_objects,
+  const ExtendedPredictedObjects & target_lane_objects,
+  const ExtendedPredictedObjects & other_lane_objects, const std::string & ns)
+{
+  int32_t update_id = 0;
+  auto current_marker =
+    marker_utils::showFilteredObjects(current_lane_objects, ns, colors::yellow(), update_id);
+  update_id += static_cast<int32_t>(current_marker.markers.size());
+  auto target_marker =
+    marker_utils::showFilteredObjects(target_lane_objects, ns, colors::aqua(), update_id);
+  update_id += static_cast<int32_t>(target_marker.markers.size());
+  auto other_marker =
+    marker_utils::showFilteredObjects(other_lane_objects, ns, colors::medium_orchid(), update_id);
+
+  MarkerArray marker_array;
+  marker_array.markers.insert(
+    marker_array.markers.end(), current_marker.markers.begin(), current_marker.markers.end());
+  marker_array.markers.insert(
+    marker_array.markers.end(), target_marker.markers.begin(), target_marker.markers.end());
+  marker_array.markers.insert(
+    marker_array.markers.end(), other_marker.markers.begin(), other_marker.markers.end());
+  return marker_array;
+}
 }  // namespace marker_utils::lane_change_markers

planning/behavior_path_planner/src/marker_utils/utils.cpp

@@ -21,6 +21,8 @@
 
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
 
+#include <cstdint>
+
 namespace marker_utils
 {
 using behavior_path_planner::ShiftLine;
@@ -645,4 +647,40 @@ MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, st
   return marker_array;
 }
 
+MarkerArray showFilteredObjects(
+  const ExtendedPredictedObjects & predicted_objects, const std::string & ns,
+  const ColorRGBA & color, int32_t id)
+{
+  using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject;
+  if (predicted_objects.empty()) {
+    return MarkerArray{};
+  }
+
+  const auto now = rclcpp::Clock{RCL_ROS_TIME}.now();
+
+  auto default_cube_marker =
+    [&](const auto & width, const auto & depth, const auto & color = colors::green()) {
+      return createDefaultMarker(
+        "map", now, ns + "_cube", ++id, visualization_msgs::msg::Marker::CUBE,
+        createMarkerScale(width, depth, 1.0), color);
+    };
+
+  MarkerArray marker_array;
+  marker_array.markers.reserve(
+    predicted_objects.size());  // poly ego, text ego, poly obj, text obj, cube obj
+
+  std::for_each(
+    predicted_objects.begin(), predicted_objects.end(), [&](const ExtendedPredictedObject & obj) {
+      const auto insert_cube_marker = [&](const auto & pose) {
+        marker_array.markers.emplace_back();
+        auto & cube_marker = marker_array.markers.back();
+        cube_marker = default_cube_marker(1.0, 1.0, color);
+        cube_marker.pose = pose;
+      };
+      insert_cube_marker(obj.initial_pose.pose);
+    });
+
+  return marker_array;
+}
+
 }  // namespace marker_utils

planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp

@@ -15,6 +15,7 @@
 #include "behavior_path_planner/scene_module/lane_change/interface.hpp"
 
 #include "behavior_path_planner/marker_utils/lane_change/debug.hpp"
+#include "behavior_path_planner/marker_utils/utils.hpp"
 #include "behavior_path_planner/scene_module/scene_module_interface.hpp"
 #include "behavior_path_planner/scene_module/scene_module_visitor.hpp"
 #include "behavior_path_planner/utils/lane_change/utils.hpp"
@@ -305,17 +306,22 @@ void LaneChangeInterface::setObjectDebugVisualization() const
   using marker_utils::showPredictedPath;
   using marker_utils::showSafetyCheckInfo;
   using marker_utils::lane_change_markers::showAllValidLaneChangePath;
+  using marker_utils::lane_change_markers::showFilteredObjects;
 
   const auto debug_data = module_type_->getDebugData();
   const auto debug_after_approval = module_type_->getAfterApprovalDebugData();
   const auto debug_valid_path = module_type_->getDebugValidPath();
+  const auto debug_filtered_objects = module_type_->getDebugFilteredObjects();
 
   debug_marker_.markers.clear();
   const auto add = [this](const MarkerArray & added) {
     tier4_autoware_utils::appendMarkerArray(added, &debug_marker_);
   };
 
   add(showAllValidLaneChangePath(debug_valid_path, "lane_change_valid_paths"));
+  add(showFilteredObjects(
+    debug_filtered_objects.current_lane, debug_filtered_objects.target_lane,
+    debug_filtered_objects.other_lane, "object_filtered"));
   if (!debug_data.empty()) {
     add(showSafetyCheckInfo(debug_data, "object_debug_info"));
     add(showPredictedPath(debug_data, "ego_predicted_path"));