[pull] main from autowarefoundation:main

common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp

@@ -40,6 +40,11 @@ visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker(
   const double longitudinal_offset = 0.0, const std::string & ns_prefix = "",
   const bool is_driving_forward = true);
 
+visualization_msgs::msg::MarkerArray createIntendedPassVirtualMarker(
+  const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now,
+  const int32_t id, const double longitudinal_offset, const std::string & ns_prefix,
+  const bool is_driving_forward);
+
 visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker(
   const rclcpp::Time & now, const int32_t id);
 

common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp

@@ -29,12 +29,13 @@ namespace autoware::motion_utils
 {
 
 /// @brief type of virtual wall associated with different marker styles and namespace
-enum VirtualWallType { stop, slowdown, deadline };
+enum VirtualWallType { stop, slowdown, deadline, pass };
 /// @brief virtual wall to be visualized in rviz
 struct VirtualWall
 {
   geometry_msgs::msg::Pose pose{};
   std::string text{};
+  std::string detail{};
   std::string ns{};
   VirtualWallType style = stop;
   double longitudinal_offset{};

common/autoware_motion_utils/src/marker/marker_helper.cpp

@@ -54,7 +54,7 @@ inline visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray(
   {
     auto marker = createDefaultMarker(
       "map", now, ns_prefix + "factor_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING,
-      createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 1.0));
+      createMarkerScale(0.0, 0.0, 1.0 /*font size*/), createMarkerColor(1.0, 1.0, 1.0, 1.0));
 
     marker.pose = vehicle_front_pose;
     marker.pose.position.z += 2.0;
@@ -85,6 +85,41 @@ inline visualization_msgs::msg::MarkerArray createDeletedVirtualWallMarkerArray(
 
   return marker_array;
 }
+
+inline visualization_msgs::msg::MarkerArray createIntendedPassArrowMarkerArray(
+  const geometry_msgs::msg::Pose & vehicle_front_pose, const std::string & module_name,
+  const std::string & ns_prefix, const rclcpp::Time & now, const int32_t id,
+  const std_msgs::msg::ColorRGBA & color)
+{
+  visualization_msgs::msg::MarkerArray marker_array;
+
+  // Arrow
+  {
+    auto marker = createDefaultMarker(
+      "map", now, ns_prefix + "direction", id, visualization_msgs::msg::Marker::ARROW,
+      createMarkerScale(2.5 /*length*/, 0.15 /*width*/, 1.0 /*height*/), color);
+
+    marker.pose = vehicle_front_pose;
+
+    marker_array.markers.push_back(marker);
+  }
+
+  // Factor Text
+  {
+    auto marker = createDefaultMarker(
+      "map", now, ns_prefix + "factor_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING,
+      createMarkerScale(0.0, 0.0, 0.4 /*font size*/), createMarkerColor(1.0, 1.0, 1.0, 1.0));
+
+    marker.pose = vehicle_front_pose;
+    marker.pose.position.z += 2.0;
+    marker.text = module_name;
+
+    marker_array.markers.push_back(marker);
+  }
+
+  return marker_array;
+}
+
 }  // namespace
 
 namespace autoware::motion_utils
@@ -125,6 +160,18 @@ visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker(
     createMarkerColor(0.0, 1.0, 0.0, 0.5));
 }
 
+visualization_msgs::msg::MarkerArray createIntendedPassVirtualMarker(
+  const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now,
+  const int32_t id, const double longitudinal_offset, const std::string & ns_prefix,
+  const bool is_driving_forward)
+{
+  const auto pose_with_offset = autoware::universe_utils::calcOffsetPose(
+    pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0);
+  return createIntendedPassArrowMarkerArray(
+    pose_with_offset, module_name, ns_prefix + "intended_pass_", now, id,
+    createMarkerColor(0.77, 0.77, 0.77, 0.5));
+}
+
 visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker(
   const rclcpp::Time & now, const int32_t id)
 {

common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp

@@ -63,10 +63,16 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::create_markers(
       case deadline:
         create_fn = autoware::motion_utils::createDeadLineVirtualWallMarker;
         break;
+      case pass:
+        create_fn = autoware::motion_utils::createIntendedPassVirtualMarker;
+        break;
     }
+    const auto wall_text = virtual_wall.detail.empty()
+                             ? virtual_wall.text
+                             : virtual_wall.text + "(" + virtual_wall.detail + ")";
     auto markers = create_fn(
-      virtual_wall.pose, virtual_wall.text, now, 0, virtual_wall.longitudinal_offset,
-      virtual_wall.ns, virtual_wall.is_driving_forward);
+      virtual_wall.pose, wall_text, now, 0, virtual_wall.longitudinal_offset, virtual_wall.ns,
+      virtual_wall.is_driving_forward);
     for (auto & marker : markers.markers) {
       marker.id = static_cast<int>(marker_count_per_namespace_[marker.ns].current++);
       marker_array.markers.push_back(marker);

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp

@@ -29,8 +29,12 @@ GeometricPullOver::GeometricPullOver(
   rclcpp::Node & node, const GoalPlannerParameters & parameters, const bool is_forward)
 : PullOverPlannerBase{node, parameters},
   parallel_parking_parameters_{parameters.parallel_parking_parameters},
-  lane_departure_checker_{
-    lane_departure_checker::Param{parameters.lane_departure_check_expansion_margin}, vehicle_info_},
+  lane_departure_checker_{[&]() {
+    auto lane_departure_checker_params = lane_departure_checker::Param{};
+    lane_departure_checker_params.footprint_extra_margin =
+      parameters.lane_departure_check_expansion_margin;
+    return LaneDepartureChecker{lane_departure_checker_params, vehicle_info_};
+  }()},
   is_forward_{is_forward},
   left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE}
 {

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp

@@ -32,8 +32,12 @@ namespace autoware::behavior_path_planner
 {
 ShiftPullOver::ShiftPullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters)
 : PullOverPlannerBase{node, parameters},
-  lane_departure_checker_{
-    lane_departure_checker::Param{parameters.lane_departure_check_expansion_margin}, vehicle_info_},
+  lane_departure_checker_{[&]() {
+    auto lane_departure_checker_params = lane_departure_checker::Param{};
+    lane_departure_checker_params.footprint_extra_margin =
+      parameters.lane_departure_check_expansion_margin;
+    return LaneDepartureChecker{lane_departure_checker_params, vehicle_info_};
+  }()},
   left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE}
 {
 }

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md

@@ -1149,7 +1149,8 @@ The following parameters are used to configure terminal lane change path feature
 | `collision_check.check_other_lanes`                      | [-]   | boolean | If true, the lane change module includes objects in other lanes when performing collision assessment.                                                                                                      | false         |
 | `collision_check.use_all_predicted_paths`                | [-]   | boolean | If false, use only the predicted path that has the maximum confidence.                                                                                                                                     | true          |
 | `collision_check.prediction_time_resolution`             | [s]   | double  | Time resolution for object's path interpolation and collision check.                                                                                                                                       | 0.5           |
-| `collision_check.yaw_diff_threshold`                     | [rad] | double  | Maximum yaw difference between ego and object when executing rss-based collision checking                                                                                                                  | 3.1416        |
+| `collision_check.yaw_diff_threshold`                     | [rad] | double  | Maximum yaw difference between predicted ego pose and predicted object pose when executing rss-based collision checking                                                                                    | 3.1416        |
+| `collision_check.th_incoming_object_yaw`                 | [rad] | double  | Maximum yaw difference between current ego pose and current object pose. Objects with a yaw difference exceeding this value are excluded from the safety check.                                            | 2.3562        |
 
 #### safety constraints during lane change path is computed
 
@@ -1162,6 +1163,7 @@ The following parameters are used to configure terminal lane change path feature
 | `safety_check.execution.lateral_distance_max_threshold`      | [m]     | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe.                | 2.0           |
 | `safety_check.execution.longitudinal_distance_min_threshold` | [m]     | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe.      | 3.0           |
 | `safety_check.execution.longitudinal_velocity_delta_time`    | [m]     | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance)                                         | 0.8           |
+| `safety_check.execution.extended_polygon_policy`             | [-]     | string | Policy used to determine the polygon shape for the safety check. Available options are: `rectangle` or `along-path`.                                           | `rectangle`   |
 
 #### safety constraints specifically for stopped or parked vehicles
 
@@ -1174,6 +1176,7 @@ The following parameters are used to configure terminal lane change path feature
 | `safety_check.parked.lateral_distance_max_threshold`      | [m]     | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe.                | 1.0           |
 | `safety_check.parked.longitudinal_distance_min_threshold` | [m]     | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe.      | 3.0           |
 | `safety_check.parked.longitudinal_velocity_delta_time`    | [m]     | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance)                                         | 0.8           |
+| `safety_check.parked.extended_polygon_policy`             | [-]     | string | Policy used to determine the polygon shape for the safety check. Available options are: `rectangle` or `along-path`.                                           | `rectangle`   |
 
 ##### safety constraints to cancel lane change path
 
@@ -1186,6 +1189,7 @@ The following parameters are used to configure terminal lane change path feature
 | `safety_check.cancel.lateral_distance_max_threshold`      | [m]     | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe.                | 1.0           |
 | `safety_check.cancel.longitudinal_distance_min_threshold` | [m]     | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe.      | 2.5           |
 | `safety_check.cancel.longitudinal_velocity_delta_time`    | [m]     | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance)                                         | 0.6           |
+| `safety_check.cancel.extended_polygon_policy`             | [-]     | string | Policy used to determine the polygon shape for the safety check. Available options are: `rectangle` or `along-path`.                                           | `rectangle`   |
 
 ##### safety constraints used during lane change path is computed when ego is stuck
 
@@ -1198,6 +1202,7 @@ The following parameters are used to configure terminal lane change path feature
 | `safety_check.stuck.lateral_distance_max_threshold`      | [m]     | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe.                | 2.0           |
 | `safety_check.stuck.longitudinal_distance_min_threshold` | [m]     | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe.      | 3.0           |
 | `safety_check.stuck.longitudinal_velocity_delta_time`    | [m]     | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance)                                         | 0.8           |
+| `safety_check.stuck.extended_polygon_policy`             | [-]     | string | Policy used to determine the polygon shape for the safety check. Available options are: `rectangle` or `along-path`.                                           | `rectangle`   |
 
 (\*1) the value must be negative.
 

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

@@ -58,6 +58,7 @@
           lateral_distance_max_threshold: 2.0
           longitudinal_distance_min_threshold: 3.0
           longitudinal_velocity_delta_time: 0.0
+          extended_polygon_policy: "rectangle"
         parked:
           expected_front_deceleration: -1.0
           expected_rear_deceleration: -2.0
@@ -66,6 +67,7 @@
           lateral_distance_max_threshold: 1.0
           longitudinal_distance_min_threshold: 3.0
           longitudinal_velocity_delta_time: 0.0
+          extended_polygon_policy: "rectangle"
         cancel:
           expected_front_deceleration: -1.0
           expected_rear_deceleration: -2.0
@@ -74,6 +76,7 @@
           lateral_distance_max_threshold: 1.0
           longitudinal_distance_min_threshold: 2.5
           longitudinal_velocity_delta_time: 0.0
+          extended_polygon_policy: "rectangle"
         stuck:
           expected_front_deceleration: -1.0
           expected_rear_deceleration: -1.0
@@ -82,6 +85,7 @@
           lateral_distance_max_threshold: 2.0
           longitudinal_distance_min_threshold: 3.0
           longitudinal_velocity_delta_time: 0.0
+          extended_polygon_policy: "rectangle"
 
         # lane expansion for object filtering
         lane_expansion:
@@ -123,7 +127,8 @@
           intersection: true
           turns: true
         prediction_time_resolution: 0.5
-        yaw_diff_threshold: 3.1416
+        th_incoming_object_yaw: 2.3562 # [rad]
+        yaw_diff_threshold: 3.1416 # [rad]
         check_current_lanes: false
         check_other_lanes: false
         use_all_predicted_paths: false

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp

@@ -94,6 +94,7 @@ struct CollisionCheckParameters
   bool check_current_lane{true};
   bool check_other_lanes{true};
   bool use_all_predicted_paths{false};
+  double th_incoming_object_yaw{2.3562};
   double th_yaw_diff{3.1416};
   double prediction_time_resolution{0.5};
 };

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp

@@ -152,6 +152,8 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s
       getOrDeclareParameter<double>(*node, parameter("collision_check.prediction_time_resolution"));
     p.safety.collision_check.th_yaw_diff =
       getOrDeclareParameter<double>(*node, parameter("collision_check.yaw_diff_threshold"));
+    p.safety.collision_check.th_incoming_object_yaw =
+      getOrDeclareParameter<double>(*node, parameter("collision_check.th_incoming_object_yaw"));
 
     // rss check
     auto set_rss_params = [&](auto & params, const std::string & prefix) {
@@ -169,6 +171,8 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s
         *node, parameter(prefix + ".rear_vehicle_safety_time_margin"));
       params.lateral_distance_max_threshold =
         getOrDeclareParameter<double>(*node, parameter(prefix + ".lateral_distance_max_threshold"));
+      params.extended_polygon_policy =
+        getOrDeclareParameter<std::string>(*node, parameter(prefix + ".extended_polygon_policy"));
     };
     set_rss_params(p.safety.rss_params, "safety_check.execution");
     set_rss_params(p.safety.rss_params_for_parked, "safety_check.parked");
@@ -454,6 +458,19 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Param
       p->safety.collision_check.prediction_time_resolution);
     updateParam<double>(
       parameters, ns + "yaw_diff_threshold", p->safety.collision_check.th_yaw_diff);
+
+    auto th_incoming_object_yaw = p->safety.collision_check.th_incoming_object_yaw;
+    updateParam<double>(parameters, ns + "th_incoming_object_yaw", th_incoming_object_yaw);
+    if (th_incoming_object_yaw >= M_PI_2) {
+      p->safety.collision_check.th_incoming_object_yaw = th_incoming_object_yaw;
+    } else {
+      RCLCPP_WARN_THROTTLE(
+        node_->get_logger(), *node_->get_clock(), 5000,
+        "The value of th_incoming_object_yaw (%.3f rad) is less than the minimum possible value "
+        "(%.3f "
+        "rad).",
+        th_incoming_object_yaw, M_PI_2);
+    }
   }
 
   {
@@ -483,7 +500,7 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Param
     updateParam<double>(parameters, ns + "stop_time", p->th_stop_time);
   }
 
-  auto update_rss_params = [&parameters](const std::string & prefix, auto & params) {
+  auto update_rss_params = [&parameters, this](const std::string & prefix, auto & params) {
     using autoware::universe_utils::updateParam;
     updateParam<double>(
       parameters, prefix + "longitudinal_distance_min_threshold",
@@ -502,6 +519,19 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Param
       params.rear_vehicle_safety_time_margin);
     updateParam<double>(
       parameters, prefix + "lateral_distance_max_threshold", params.lateral_distance_max_threshold);
+
+    auto extended_polygon_policy = params.extended_polygon_policy;
+    updateParam<std::string>(
+      parameters, prefix + "extended_polygon_policy", extended_polygon_policy);
+    if (extended_polygon_policy == "rectangle" || extended_polygon_policy == "along_path") {
+      params.extended_polygon_policy = extended_polygon_policy;
+    } else {
+      RCLCPP_WARN_THROTTLE(
+        node_->get_logger(), *node_->get_clock(), 1000,
+        "Policy %s not supported or there's typo. Make sure you choose either 'rectangle' or "
+        "'along_path'",
+        extended_polygon_policy.c_str());
+    }
   };
 
   update_rss_params("lane_change.safety_check.execution.", p->safety.rss_params);

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

@@ -1051,7 +1051,9 @@ void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const
 
   const auto is_same_direction = [&](const PredictedObject & object) {
     const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose;
-    return !utils::path_safety_checker::isTargetObjectOncoming(current_pose, object_pose);
+    return !utils::path_safety_checker::isTargetObjectOncoming(
+      current_pose, object_pose,
+      common_data_ptr_->lc_param_ptr->safety.collision_check.th_incoming_object_yaw);
   };
 
   //  Perception noise could make stationary objects seem opposite the ego vehicle; check the
@@ -1792,7 +1794,6 @@ bool NormalLaneChange::is_colliding(
 
   constexpr auto is_safe{true};
   auto current_debug_data = utils::path_safety_checker::createObjectDebug(obj);
-  constexpr auto collision_check_yaw_diff_threshold{M_PI};
   constexpr auto hysteresis_factor{1.0};
   const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj(
     obj, lane_change_parameters_->safety.collision_check.use_all_predicted_paths);
@@ -1817,7 +1818,8 @@ bool NormalLaneChange::is_colliding(
     const auto collided_polygons = utils::path_safety_checker::get_collided_polygons(
       lane_change_path.path, ego_predicted_path, obj, predicted_obj_path, bpp_param.vehicle_info,
       selected_rss_param, hysteresis_factor, safety_check_max_vel,
-      collision_check_yaw_diff_threshold, current_debug_data.second);
+      common_data_ptr_->lc_param_ptr->safety.collision_check.th_yaw_diff,
+      current_debug_data.second);
 
     if (collided_polygons.empty()) {
       utils::path_safety_checker::updateCollisionCheckDebugMap(