feat(avoidance): improve avoidance target filter (autowarefoundation#…

…2329)

* feat(route_handler): add getMostLeftLanelet()

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* feat(avoidance): calc shiftable ratio in avoidance target filtering process

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* feat(avoidance): output object's debug info for rviz

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(avoidance): use avoidance debug factor

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* feat(tier4_planning_launch): add new params for avoidance

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(avoidance): reorder params for readability

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(tier4_planning_launch): reorder params for readability

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
Signed-off-by: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>

launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml

@@ -11,11 +11,19 @@
       enable_avoidance_over_opposite_direction: true
       enable_update_path_when_object_is_gone: false
 
-      threshold_distance_object_is_on_center: 1.0 # [m]
+      # For target object filtering
       threshold_speed_object_is_stopped: 1.0      # [m/s]
       threshold_time_object_is_moving: 1.0        # [s]
+
       object_check_forward_distance: 150.0        # [m]
       object_check_backward_distance: 2.0         # [m]
+
+      threshold_distance_object_is_on_center: 1.0 # [m]
+
+      object_check_shiftable_ratio: 0.6           # [-]
+      object_check_min_road_shoulder_width: 0.5   # [m]
+
+      # For lateral margin
       object_envelope_buffer: 0.3                 # [m]
       lateral_collision_margin: 1.0               # [m]
       lateral_collision_safety_buffer: 0.7        # [m]

planning/behavior_path_planner/config/avoidance/avoidance.param.yaml

@@ -11,10 +11,20 @@
       enable_avoidance_over_opposite_direction: true
       enable_update_path_when_object_is_gone: false
 
-      threshold_distance_object_is_on_center: 1.0 # [m]
+      # For target object filtering
       threshold_speed_object_is_stopped: 1.0      # [m/s]
+      threshold_time_object_is_moving: 1.0        # [s]
+
       object_check_forward_distance: 150.0        # [m]
       object_check_backward_distance: 2.0         # [m]
+
+      threshold_distance_object_is_on_center: 1.0 # [m]
+
+      object_check_shiftable_ratio: 0.6           # [-]
+      object_check_min_road_shoulder_width: 0.5   # [m]
+
+      # For lateral margin
+      object_envelope_buffer: 0.3                 # [m]
       lateral_collision_margin: 1.0               # [m]
       lateral_collision_safety_buffer: 0.7        # [m]
 

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp

@@ -35,6 +35,7 @@ namespace behavior_path_planner
 using autoware_auto_perception_msgs::msg::PredictedObject;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 
+using tier4_autoware_utils::Point2d;
 using tier4_autoware_utils::Polygon2d;
 using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
 
@@ -80,6 +81,12 @@ struct AvoidanceParameters
   // continue to detect backward vehicles as avoidance targets until they are this distance away
   double object_check_backward_distance;
 
+  // use in judge whether the vehicle is parking object on road shoulder
+  double object_check_shiftable_ratio;
+
+  // minimum road shoulder width. maybe 0.5 [m]
+  double object_check_min_road_shoulder_width;
+
   // object's enveloped polygon
   double object_envelope_buffer;
 
@@ -201,6 +208,9 @@ struct ObjectData  // avoidance target
   // lateral distance to the closest footprint, in Frenet coordinate
   double overhang_dist;
 
+  // lateral shiftable ratio
+  double shiftable_ratio{0.0};
+
   // count up when object disappeared. Removed when it exceeds threshold.
   rclcpp::Time last_seen;
   double lost_time{0.0};
@@ -218,6 +228,9 @@ struct ObjectData  // avoidance target
   // envelope polygon
   Polygon2d envelope_poly{};
 
+  // envelope polygon centroid
+  Point2d centroid{};
+
   // lateral distance from overhang to the road shoulder
   double to_road_shoulder_distance{0.0};
 };

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp

@@ -37,6 +37,7 @@ namespace marker_utils::avoidance_marker
 using autoware_auto_perception_msgs::msg::PredictedObjects;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using behavior_path_planner::AvoidLineArray;
+using behavior_path_planner::ObjectDataArray;
 using behavior_path_planner::ShiftLineArray;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Polygon;

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -285,6 +285,8 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
   p.threshold_time_object_is_moving = dp("threshold_time_object_is_moving", 1.0);
   p.object_check_forward_distance = dp("object_check_forward_distance", 150.0);
   p.object_check_backward_distance = dp("object_check_backward_distance", 2.0);
+  p.object_check_shiftable_ratio = dp("object_check_shiftable_ratio", 1.0);
+  p.object_check_min_road_shoulder_width = dp("object_check_min_road_shoulder_width", 0.5);
   p.object_envelope_buffer = dp("object_envelope_buffer", 0.1);
   p.lateral_collision_margin = dp("lateral_collision_margin", 2.0);
   p.lateral_collision_safety_buffer = dp("lateral_collision_safety_buffer", 0.5);

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -191,7 +191,10 @@ AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & deb
 void AvoidanceModule::fillAvoidanceTargetObjects(
   AvoidancePlanningData & data, DebugData & debug) const
 {
+  using boost::geometry::return_centroid;
+  using boost::geometry::within;
   using lanelet::geometry::distance2d;
+  using lanelet::geometry::toArcCoordinates;
   using lanelet::utils::getId;
   using lanelet::utils::to2D;
 
@@ -224,7 +227,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects(
   std::vector<AvoidanceDebugMsg> avoidance_debug_msg_array;
   for (const auto & i : lane_filtered_objects_index) {
     const auto & object = planner_data_->dynamic_object->objects.at(i);
-    const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position;
+    const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose;
     AvoidanceDebugMsg avoidance_debug_msg;
     const auto avoidance_debug_array_false_and_push_back =
       [&avoidance_debug_msg, &avoidance_debug_msg_array](const std::string & failed_reason) {
@@ -242,12 +245,15 @@ void AvoidanceModule::fillAvoidanceTargetObjects(
     object_data.object = object;
     avoidance_debug_msg.object_id = getUuidStr(object_data);
 
-    const auto object_closest_index = findNearestIndex(path_points, object_pos);
+    const auto object_closest_index = findNearestIndex(path_points, object_pose.position);
     const auto object_closest_pose = path_points.at(object_closest_index).point.pose;
 
     // Calc envelop polygon.
     fillObjectEnvelopePolygon(object_closest_pose, object_data);
 
+    // calc object centroid.
+    object_data.centroid = return_centroid<Point2d>(object_data.envelope_poly);
+
     // calc longitudinal distance from ego to closest target object footprint point.
     fillLongitudinalAndLengthByClosestEnvelopeFootprint(data.reference_path, ego_pos, object_data);
     avoidance_debug_msg.longitudinal_distance = object_data.longitudinal;
@@ -256,7 +262,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects(
     fillObjectMovingTime(object_data);
 
     if (object_data.move_time > parameters_->threshold_time_object_is_moving) {
-      avoidance_debug_array_false_and_push_back("MovingObject");
+      avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::MOVING_OBJECT);
       data.other_objects.push_back(object_data);
       continue;
     }
@@ -281,7 +287,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects(
     }
 
     // Calc lateral deviation from path to target object.
-    object_data.lateral = calcLateralDeviation(object_closest_pose, object_pos);
+    object_data.lateral = calcLateralDeviation(object_closest_pose, object_pose.position);
     avoidance_debug_msg.lateral_distance_from_centerline = object_data.lateral;
 
     // Find the footprint point closest to the path, set to object_data.overhang_distance.
@@ -332,6 +338,89 @@ void AvoidanceModule::fillAvoidanceTargetObjects(
       continue;
     }
 
+    lanelet::ConstLanelet object_closest_lanelet;
+    const auto lanelet_map = rh->getLaneletMapPtr();
+    if (!lanelet::utils::query::getClosestLanelet(
+          lanelet::utils::query::laneletLayer(lanelet_map), object_pose, &object_closest_lanelet)) {
+      continue;
+    }
+
+    lanelet::BasicPoint2d object_centroid(object_data.centroid.x(), object_data.centroid.y());
+
+    /**
+     * Is not object in adjacent lane?
+     *   - Yes -> Is parking object?
+     *     - Yes -> the object is avoidance target.
+     *     - No -> ignore this object.
+     *   - No -> the object is avoidance target no matter whether it is parking object or not.
+     */
+    const auto is_in_ego_lane =
+      within(object_centroid, overhang_lanelet.polygon2d().basicPolygon());
+    if (is_in_ego_lane) {
+      /**
+       * TODO(Satoshi Ota) use intersection area
+       * under the assumption that there is no parking vehicle inside intersection,
+       * ignore all objects that is in the ego lane as not parking objects.
+       */
+      std::string turn_direction = overhang_lanelet.attributeOr("turn_direction", "else");
+      if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") {
+        avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::NOT_PARKING_OBJECT);
+        data.other_objects.push_back(object_data);
+        continue;
+      }
+
+      const auto centerline_pose =
+        lanelet::utils::getClosestCenterPose(object_closest_lanelet, object_pose.position);
+      lanelet::BasicPoint3d centerline_point(
+        centerline_pose.position.x, centerline_pose.position.y, centerline_pose.position.z);
+
+      // ============================================ <- most_left_lanelet.leftBound()
+      // y              road shoulder
+      // ^ ------------------------------------------
+      // |   x                                +
+      // +---> --- object closest lanelet --- o ----- <- object_closest_lanelet.centerline()
+      //
+      // --------------------------------------------
+      // +: object position
+      // o: nearest point on centerline
+
+      const auto most_left_road_lanelet = rh->getMostLeftLanelet(object_closest_lanelet);
+      const auto most_left_lanelet_candidates =
+        rh->getLaneletMapPtr()->laneletLayer.findUsages(most_left_road_lanelet.leftBound());
+
+      lanelet::ConstLanelet most_left_lanelet = most_left_road_lanelet;
+
+      for (const auto & ll : most_left_lanelet_candidates) {
+        const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype);
+        if (sub_type.value() == "road_shoulder") {
+          most_left_lanelet = ll;
+        }
+      }
+
+      const auto center_to_left_boundary =
+        distance2d(to2D(most_left_lanelet.leftBound().basicLineString()), to2D(centerline_point));
+      double object_shiftable_distance = center_to_left_boundary - 0.5 * object.shape.dimensions.y;
+
+      const lanelet::Attribute sub_type =
+        most_left_lanelet.attribute(lanelet::AttributeName::Subtype);
+      if (sub_type.value() != "road_shoulder") {
+        object_shiftable_distance += parameters_->object_check_min_road_shoulder_width;
+      }
+
+      const auto arc_coordinates = toArcCoordinates(
+        to2D(object_closest_lanelet.centerline().basicLineString()), object_centroid);
+      object_data.shiftable_ratio = arc_coordinates.distance / object_shiftable_distance;
+
+      const auto is_parking_object =
+        object_data.shiftable_ratio > parameters_->object_check_shiftable_ratio;
+
+      if (!is_parking_object) {
+        avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::NOT_PARKING_OBJECT);
+        data.other_objects.push_back(object_data);
+        continue;
+      }
+    }
+
     object_data.last_seen = clock_->now();
 
     // set data
@@ -615,7 +704,7 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(const ObjectDataArr
     const auto is_object_on_right = isOnRight(o);
     const auto shift_length = getShiftLength(o, is_object_on_right, avoid_margin);
     if (isSameDirectionShift(is_object_on_right, shift_length)) {
-      avoidance_debug_array_false_and_push_back("IgnoreSameDirectionShift");
+      avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::SAME_DIRECTION_SHIFT);
       continue;
     }