feat(behavior_velocitiy_planner): predict front vehicle deceleration …

…in intersection and temporarily stop (tier4#1194)

* calculating stopping distance for frontcar from estimated velocity

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

* calc stopping_point_projected and stopping_point along centerline

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

* create stuck_vehicle_detect_area in modifyVelocity(TODO: use pose of frontcar at stopping_position

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

* use centerline on ego_lane_with_next_lane

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

* properly checking if stopping_point is in stuck_vehicle_detect_area

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

* also check if the point behind stopping_point is in detection_area(aka attention area)

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

* refactored

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

* will return collision_deteced if is_in_sturck_area && is_behind_point_in_detection_area

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

* look working

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

* refactored, rename parameter

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

* added flag

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

* fixed the order of isAheadOf, working in scenario test as well

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

* added description in stuck vehicle detection section

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

* reflected comments: (1) use vector of ids (2) changed intersection.param.yaml

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml

@@ -16,6 +16,8 @@
       collision_start_margin_time: 5.0 # [s]
       collision_end_margin_time: 2.0 # [s]
       use_stuck_stopline: false # stopline generate before the intersection lanelet when is_stuck
+      assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning
+      enable_front_car_decel_prediction: false # By default this feature is disabled
 
     merge_from_private_road:
       stop_duration_sec: 1.0

planning/behavior_velocity_planner/config/intersection.param.yaml

@@ -16,6 +16,8 @@
       collision_start_margin_time: 5.0 # [s]
       collision_end_margin_time: 2.0 # [s]
       use_stuck_stopline: false # stopline generate before the intersection lanelet when is_stuck
+      assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning
+      enable_front_car_decel_prediction: false # By default this feature is disabled
 
     merge_from_private_road:
       stop_duration_sec: 1.0

planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp

@@ -97,6 +97,7 @@ class IntersectionModule : public SceneModuleInterface
     std::vector<lanelet::CompoundPolygon3d> detection_area;
     autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets;
     autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets;
+    geometry_msgs::msg::Pose predicted_obj_pose;
   };
 
 public:
@@ -123,6 +124,9 @@ class IntersectionModule : public SceneModuleInterface
     double collision_start_margin_time;  //! start margin time to check collision
     double collision_end_margin_time;    //! end margin time to check collision
     bool use_stuck_stopline;  //! stopline generate before the intersection lanelet when is_stuck.
+    double
+      assumed_front_car_decel;  //! the expected deceleration of front car when front car as well
+    bool enable_front_car_decel_prediction;  //! flag for using above feature
   };
 
   IntersectionModule(
@@ -166,7 +170,7 @@ class IntersectionModule : public SceneModuleInterface
     const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
     const std::vector<int> & detection_area_lanelet_ids,
     const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr,
-    const int closest_idx);
+    const int closest_idx, const Polygon2d & stuck_vehicle_detect_area);
 
   /**
    * @brief Check if there is a stopped vehicle on the ego-lane.
@@ -177,10 +181,8 @@ class IntersectionModule : public SceneModuleInterface
    * @return true if exists
    */
   bool checkStuckVehicleInIntersection(
-    lanelet::LaneletMapConstPtr lanelet_map_ptr,
-    const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx,
-    const int stop_idx,
-    const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr) const;
+    const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr,
+    const Polygon2d & stuck_vehicle_detect_area) const;
 
   /**
    * @brief Calculate the polygon of the path from the ego-car position to the end of the
@@ -289,6 +291,19 @@ class IntersectionModule : public SceneModuleInterface
     lanelet::LaneletMapConstPtr lanelet_map_ptr,
     const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx) const;
 
+  /**
+   * @brief Check if the ego is expected to stop in the opposite lane if the front vehicle starts
+   * deceleration from current velocity and stop befor the crosswalk. If the stop position of front
+   * vehicle is in stuck area, and the position `ego_length` meter behind is in detection area,
+   * return true
+   * @param ego_poly Polygon of ego_with_next_lane
+   * @param closest_lanelet
+   * @return true if the ego is expected to stop in the opposite lane
+   */
+  bool checkFrontVehicleDeceleration(
+    lanelet::ConstLanelets & ego_lane_with_next_lane, lanelet::ConstLanelet & closest_lanelet,
+    const Polygon2d & stuck_vehicle_detect_area,
+    const autoware_auto_perception_msgs::msg::PredictedObject & object) const;
   StateMachine state_machine_;  //! for state
 
   // Debug

planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp

@@ -126,6 +126,9 @@ double calcArcLengthFromPath(
 
 lanelet::ConstLanelet generateOffsetLanelet(
   const lanelet::ConstLanelet lanelet, double right_margin, double left_margin);
+
+geometry_msgs::msg::Pose toPose(const geometry_msgs::msg::Point & p);
+
 }  // namespace util
 }  // namespace behavior_velocity_planner
 

planning/behavior_velocity_planner/intersection-design.md

@@ -81,23 +81,26 @@ To prevent going over the pass judge line before the traffic light stop line, th
 
 If there is any object in a certain distance (default : 5[m]) from the end point of the intersection lane on the driving lane and the object velocity is less than a threshold (default 3.0[km/h]), the object is regarded as a stuck vehicle. If the stuck vehicle exists, the ego vehicle cannot enter the intersection.
 
+As a related case, if the object in front of the ego vehicle is turning the same direction, this module predicts the stopping point that the object will reach at a certain deceleration (default: -1.0[m/s^2]). If the predicted position is in stuck vehicle detection area AND the position which `vehicle length` [m] behind the predicted position is in detection area, the ego vehicle will also stop.
+
 ### Module Parameters
 
-| Parameter                                     | Type   | Description                                                                   |
-| --------------------------------------------- | ------ | ----------------------------------------------------------------------------- |
-| `intersection/state_transit_margin_time`      | double | [m] time margin to change state                                               |
-| `intersection/path_expand_width`              | bool   | [m] path area to see with expansion                                           |
-| `intersection/stop_line_margin`               | double | [m] margin before stop line                                                   |
-| `intersection/stuck_vehicle_detect_dist`      | double | [m] this should be the length between cars when they are stopped.             |
-| `intersection/stuck_vehicle_ignore_dist`      | double | [m] obstacle stop max distance(5.0[m]) + stuck vehicle size / 2.0[m])         |
-| `intersection/stuck_vehicle_vel_thr`          | double | [m/s] velocity below 3[km/h] is ignored by default                            |
-| `intersection/intersection_velocity`          | double | [m/s] velocity to pass intersection. 10[km/h] is by default                   |
-| `intersection/intersection_max_accel`         | double | [m/s^2] acceleration in intersection                                          |
-| `intersection/detection_area_margin`          | double | [m] range for expanding detection area                                        |
-| `intersection/detection_area_length`          | double | [m] range for lidar detection 200[m] is by default                            |
-| `intersection/detection_area_angle_threshold` | double | [rad] threshold of angle difference between the detection object and lane     |
-| `intersection/min_predicted_path_confidence`  | double | [-] minimum confidence value of predicted path to use for collision detection |
-| `merge_from_private_road/stop_duration_sec`   | double | [s] duration to stop                                                          |
+| Parameter                                     | Type   | Description                                                                                    |
+| --------------------------------------------- | ------ | ---------------------------------------------------------------------------------------------- |
+| `intersection/state_transit_margin_time`      | double | [m] time margin to change state                                                                |
+| `intersection/path_expand_width`              | bool   | [m] path area to see with expansion                                                            |
+| `intersection/stop_line_margin`               | double | [m] margin before stop line                                                                    |
+| `intersection/stuck_vehicle_detect_dist`      | double | [m] this should be the length between cars when they are stopped.                              |
+| `intersection/stuck_vehicle_ignore_dist`      | double | [m] obstacle stop max distance(5.0[m]) + stuck vehicle size / 2.0[m])                          |
+| `intersection/stuck_vehicle_vel_thr`          | double | [m/s] velocity below 3[km/h] is ignored by default                                             |
+| `intersection/intersection_velocity`          | double | [m/s] velocity to pass intersection. 10[km/h] is by default                                    |
+| `intersection/intersection_max_accel`         | double | [m/s^2] acceleration in intersection                                                           |
+| `intersection/detection_area_margin`          | double | [m] range for expanding detection area                                                         |
+| `intersection/detection_area_length`          | double | [m] range for lidar detection 200[m] is by default                                             |
+| `intersection/detection_area_angle_threshold` | double | [rad] threshold of angle difference between the detection object and lane                      |
+| `intersection/min_predicted_path_confidence`  | double | [-] minimum confidence value of predicted path to use for collision detection                  |
+| `merge_from_private_road/stop_duration_sec`   | double | [s] duration to stop                                                                           |
+| `assumed_front_car_decel: 1.0`                | double | [m/s^2] deceleration of front car used to check if it could stop in the stuck area at the exit |
 
 ### How To Tune Parameters
 

planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp

@@ -244,6 +244,11 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray(
     createObjectsMarkerArray(debug_data_.stuck_targets, "stuck_targets", lane_id_, 0.99, 0.99, 0.2),
     &debug_marker_array, current_time);
 
+  appendMarkerArray(
+    createPoseMarkerArray(
+      debug_data_.predicted_obj_pose, "predicted_obj_pose", lane_id_, 0.7, 0.85, 0.9),
+    &debug_marker_array, current_time);
+
   if (state == IntersectionModule::State::STOP) {
     appendMarkerArray(
       createPoseMarkerArray(

planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp

@@ -51,6 +51,9 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
   ip.collision_start_margin_time = node.declare_parameter(ns + ".collision_start_margin_time", 5.0);
   ip.collision_end_margin_time = node.declare_parameter(ns + ".collision_end_margin_time", 2.0);
   ip.use_stuck_stopline = node.declare_parameter(ns + ".use_stuck_stopline", true);
+  ip.assumed_front_car_decel = node.declare_parameter(ns + ".assumed_front_car_decel", 1.0);
+  ip.enable_front_car_decel_prediction =
+    node.declare_parameter(ns + ".enable_front_car_decel_prediction", false);
 }
 
 MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node)

planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include <lanelet2_extension/regulatory_elements/road_marking.hpp>
+#include <lanelet2_extension/utility/message_conversion.hpp>
 #include <lanelet2_extension/utility/query.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
 #include <scene_module/intersection/scene_intersection.hpp>
@@ -171,10 +172,15 @@ bool IntersectionModule::modifyPathVelocity(
   const auto objects_ptr = planner_data_->predicted_objects;
 
   /* calculate dynamic collision around detection area */
-  bool has_collision =
-    checkCollision(lanelet_map_ptr, *path, detection_area_lanelet_ids, objects_ptr, closest_idx);
-  bool is_stuck = checkStuckVehicleInIntersection(
-    lanelet_map_ptr, *path, closest_idx, stop_line_idx, objects_ptr);
+  const double detect_length =
+    planner_param_.stuck_vehicle_detect_dist + planner_data_->vehicle_info_.vehicle_length_m;
+  const auto stuck_vehicle_detect_area = generateEgoIntersectionLanePolygon(
+    lanelet_map_ptr, *path, closest_idx, stop_line_idx, detect_length,
+    planner_param_.stuck_vehicle_detect_dist);
+  bool is_stuck = checkStuckVehicleInIntersection(objects_ptr, stuck_vehicle_detect_area);
+  bool has_collision = checkCollision(
+    lanelet_map_ptr, *path, detection_area_lanelet_ids, objects_ptr, closest_idx,
+    stuck_vehicle_detect_area);
   bool is_entry_prohibited = (has_collision || is_stuck);
   if (external_go) {
     is_entry_prohibited = false;
@@ -256,14 +262,19 @@ bool IntersectionModule::checkCollision(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   const std::vector<int> & detection_area_lanelet_ids,
   const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr,
-  const int closest_idx)
+  const int closest_idx, const Polygon2d & stuck_vehicle_detect_area)
 {
   using lanelet::utils::getArcCoordinates;
   using lanelet::utils::getPolygonFromArcLength;
 
   /* generate ego-lane polygon */
   const auto ego_poly =
     generateEgoIntersectionLanePolygon(lanelet_map_ptr, path, closest_idx, closest_idx, 0.0, 0.0);
+  lanelet::ConstLanelets ego_lane_with_next_lane = getEgoLaneWithNextLane(lanelet_map_ptr, path);
+  lanelet::ConstLanelet closest_lanelet;
+  lanelet::utils::query::getClosestLanelet(
+    ego_lane_with_next_lane, tier4_autoware_utils::getPose(path.points.at(closest_idx).point),
+    &closest_lanelet);
 
   debug_data_.ego_lane_polygon = toGeomMsg(ego_poly);
 
@@ -276,11 +287,18 @@ bool IntersectionModule::checkCollision(
       continue;
     }
 
-    // ignore vehicle in ego-lane. (TODO update check algorithm)
+    // ignore vehicle in ego-lane && behind ego
     const auto object_pose = object.kinematics.initial_pose_with_covariance.pose;
     const bool is_in_ego_lane = bg::within(to_bg2d(object_pose.position), ego_poly);
     if (is_in_ego_lane) {
-      continue;  // TODO(Kenji Miyake): check direction?
+      if (!planning_utils::isAheadOf(object_pose, planner_data_->current_pose.pose)) {
+        continue;
+      }
+      if (
+        planner_param_.enable_front_car_decel_prediction &&
+        checkFrontVehicleDeceleration(
+          ego_lane_with_next_lane, closest_lanelet, stuck_vehicle_detect_area, object))
+        return true;
     }
 
     // check direction of objects
@@ -297,7 +315,6 @@ bool IntersectionModule::checkCollision(
   const double passing_time = time_distance_array.back().first;
   cutPredictPathWithDuration(&target_objects, passing_time);
 
-  lanelet::ConstLanelets ego_lane_with_next_lane = getEgoLaneWithNextLane(lanelet_map_ptr, path);
   const auto closest_arc_coords = getArcCoordinates(
     ego_lane_with_next_lane, tier4_autoware_utils::getPose(path.points.at(closest_idx).point));
   const double distance_until_intersection =
@@ -478,16 +495,9 @@ TimeDistanceArray IntersectionModule::calcIntersectionPassingTime(
 }
 
 bool IntersectionModule::checkStuckVehicleInIntersection(
-  lanelet::LaneletMapConstPtr lanelet_map_ptr,
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx,
-  const int stop_idx,
-  const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr) const
+  const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr,
+  const Polygon2d & stuck_vehicle_detect_area) const
 {
-  const double detect_length =
-    planner_param_.stuck_vehicle_detect_dist + planner_data_->vehicle_info_.vehicle_length_m;
-  const auto stuck_vehicle_detect_area = generateEgoIntersectionLanePolygon(
-    lanelet_map_ptr, path, closest_idx, stop_idx, detect_length,
-    planner_param_.stuck_vehicle_ignore_dist);
   debug_data_.stuck_vehicle_detect_area = toGeomMsg(stuck_vehicle_detect_area);
 
   for (const auto & object : objects_ptr->objects) {
@@ -681,4 +691,77 @@ double IntersectionModule::calcDistanceUntilIntersectionLanelet(
   return distance;
 }
 
+bool IntersectionModule::checkFrontVehicleDeceleration(
+  lanelet::ConstLanelets & ego_lane_with_next_lane, lanelet::ConstLanelet & closest_lanelet,
+  const Polygon2d & stuck_vehicle_detect_area,
+  const autoware_auto_perception_msgs::msg::PredictedObject & object) const
+{
+  const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose;
+  // consider vehicle in ego-lane && in front of ego
+  const auto lon_vel = object.kinematics.initial_twist_with_covariance.twist.linear.x;
+  const double object_decel = planner_param_.assumed_front_car_decel;  // NOTE: this is positive
+  const double stopping_distance = lon_vel * lon_vel / (2 * object_decel);
+
+  std::vector<geometry_msgs::msg::Point> center_points;
+  for (auto && p : ego_lane_with_next_lane[0].centerline())
+    center_points.push_back(std::move(lanelet::utils::conversion::toGeomMsgPt(p)));
+  for (auto && p : ego_lane_with_next_lane[1].centerline())
+    center_points.push_back(std::move(lanelet::utils::conversion::toGeomMsgPt(p)));
+  const double lat_offset =
+    std::fabs(motion_utils::calcLateralOffset(center_points, object_pose.position));
+  // get the nearest centerpoint to object
+  std::vector<double> dist_obj_center_points;
+  for (const auto & p : center_points)
+    dist_obj_center_points.push_back(tier4_autoware_utils::calcDistance2d(object_pose.position, p));
+  const int obj_closest_centerpoint_idx = std::distance(
+    dist_obj_center_points.begin(),
+    std::min_element(dist_obj_center_points.begin(), dist_obj_center_points.end()));
+  // find two center_points whose distances from `closest_centerpoint` cross stopping_distance
+  double acc_dist_prev = 0.0, acc_dist = 0.0;
+  auto p1 = center_points[obj_closest_centerpoint_idx];
+  auto p2 = center_points[obj_closest_centerpoint_idx];
+  for (unsigned i = obj_closest_centerpoint_idx; i < center_points.size() - 1; ++i) {
+    p1 = center_points[i];
+    p2 = center_points[i + 1];
+    acc_dist_prev = acc_dist;
+    const auto arc_position_p1 =
+      lanelet::utils::getArcCoordinates(ego_lane_with_next_lane, util::toPose(p1));
+    const auto arc_position_p2 =
+      lanelet::utils::getArcCoordinates(ego_lane_with_next_lane, util::toPose(p2));
+    const double delta = arc_position_p2.length - arc_position_p1.length;
+    acc_dist += delta;
+    if (acc_dist > stopping_distance) {
+      break;
+    }
+  }
+  // if stopping_distance >= center_points, stopping_point is center_points[end]
+  const double ratio = (acc_dist <= stopping_distance)
+                         ? 0.0
+                         : (acc_dist - stopping_distance) / (stopping_distance - acc_dist_prev);
+  // linear interpolation
+  geometry_msgs::msg::Point stopping_point;
+  stopping_point.x = (p1.x * ratio + p2.x) / (1 + ratio);
+  stopping_point.y = (p1.y * ratio + p2.y) / (1 + ratio);
+  stopping_point.z = (p1.z * ratio + p2.z) / (1 + ratio);
+  const double lane_yaw = lanelet::utils::getLaneletAngle(closest_lanelet, stopping_point);
+  stopping_point.x += lat_offset * std::cos(lane_yaw + M_PI / 2.0);
+  stopping_point.y += lat_offset * std::sin(lane_yaw + M_PI / 2.0);
+
+  // calculate footprint of predicted stopping pose
+  autoware_auto_perception_msgs::msg::PredictedObject predicted_object = object;
+  predicted_object.kinematics.initial_pose_with_covariance.pose.position = stopping_point;
+  predicted_object.kinematics.initial_pose_with_covariance.pose.orientation =
+    tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw);
+  Polygon2d predicted_obj_footprint = toFootprintPolygon(predicted_object);
+  const bool is_in_stuck_area = !bg::disjoint(predicted_obj_footprint, stuck_vehicle_detect_area);
+  debug_data_.predicted_obj_pose.position = stopping_point;
+  debug_data_.predicted_obj_pose.orientation =
+    tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw);
+
+  if (is_in_stuck_area) {
+    return true;
+  }
+  return false;
+}
+
 }  // namespace behavior_velocity_planner

planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp

@@ -512,6 +512,13 @@ lanelet::ConstLanelet generateOffsetLanelet(
   return std::move(lanelet_with_margin);
 }
 
+geometry_msgs::msg::Pose toPose(const geometry_msgs::msg::Point & p)
+{
+  geometry_msgs::msg::Pose pose;
+  pose.position = p;
+  return pose;
+}
+
 bool generateStopLineBeforeIntersection(
   const int lane_id, lanelet::LaneletMapConstPtr lanelet_map_ptr,
   const std::shared_ptr<const PlannerData> & planner_data,