fix(out_of_lane): improve stop decision

planning/behavior_velocity_out_of_lane_module/CMakeLists.txt

@@ -6,13 +6,7 @@ autoware_package()
 pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml)
 
 ament_auto_add_library(${PROJECT_NAME} SHARED
-  src/debug.cpp
-  src/decisions.cpp
-  src/footprint.cpp
-  src/lanelets_selection.cpp
-  src/manager.cpp
-  src/overlapping_range.cpp
-  src/scene_out_of_lane.cpp
+  DIRECTORY src
 )
 
 ament_auto_package(INSTALL_TO_SHARE config)

planning/behavior_velocity_out_of_lane_module/README.md

@@ -149,14 +149,13 @@ Moreover, parameter `action.distance_buffer` adds an extra distance between the
 | `minimum_distance` | double | [m] minimum distance inside a lanelet for an overlap to be considered                                |
 | `extra_length`     | double | [m] extra arc length to add to the front and back of an overlap (used to calculate enter/exit times) |
 
-| Parameter /action             | Type   | Description                                                                                                                                                                                                              |
-| ----------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ |
-| `skip_if_over_max_decel`      | bool   | [-] if true, do not take an action that would cause more deceleration than the maximum allowed                                                                                                                           |
-| `strict`                      | bool   | [-] if true, when a decision is taken to avoid entering a lane, the stop point will make sure no lane at all is entered by ego; if false, ego stops just before entering a lane but may then be overlapping another lane |
-| `distance_buffer`             | double | [m] buffer distance to try to keep between the ego footprint and lane                                                                                                                                                    |
-| `slowdown.distance_threshold` | double | [m] insert a slow down when closer than this distance from an overlap                                                                                                                                                    |
-| `slowdown.velocity`           | double | [m] slow down velocity                                                                                                                                                                                                   |
-| `stop.distance_threshold`     | double | [m] insert a stop when closer than this distance from an overlap                                                                                                                                                         |
+| Parameter /action             | Type   | Description                                                                                    |
+| ----------------------------- | ------ | ---------------------------------------------------------------------------------------------- |
+| `skip_if_over_max_decel`      | bool   | [-] if true, do not take an action that would cause more deceleration than the maximum allowed |
+| `distance_buffer`             | double | [m] buffer distance to try to keep between the ego footprint and lane                          |
+| `slowdown.distance_threshold` | double | [m] insert a slow down when closer than this distance from an overlap                          |
+| `slowdown.velocity`           | double | [m] slow down velocity                                                                         |
+| `stop.distance_threshold`     | double | [m] insert a stop when closer than this distance from an overlap                               |
 
 | Parameter /ego       | Type   | Description                                          |
 | -------------------- | ------ | ---------------------------------------------------- |

planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml

@@ -24,8 +24,7 @@
 
       action:  # action to insert in the path if an object causes a conflict at an overlap
         skip_if_over_max_decel: true  # if true, do not take an action that would cause more deceleration than the maximum allowed
-        strict: true # if true, when a decision is taken to avoid entering a lane, the stop point will make sure no lane at all is entered by ego
-                     # if false, ego stops just before entering a lane but may then be overlapping another lane.
+        precision: 0.1  # [m] precision when inserting a stop pose in the path
         distance_buffer: 1.5  # [m] buffer distance to try to keep between the ego footprint and lane
         slowdown:
           distance_threshold: 30.0 # [m] insert a slowdown when closer than this distance from an overlap
@@ -34,6 +33,7 @@
           distance_threshold: 15.0 # [m] insert a stop when closer than this distance from an overlap
 
       ego:
+        min_assumed_velocity: 2.0 # [m/s] minimum velocity used to calculate the enter and exit times of ego
         extra_front_offset: 0.0 # [m] extra front distance
         extra_rear_offset: 0.0 # [m] extra rear distance
         extra_right_offset: 0.0 # [m] extra right distance

planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp

@@ -0,0 +1,84 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef CALCULATE_SLOWDOWN_POINTS_HPP_
+#define CALCULATE_SLOWDOWN_POINTS_HPP_
+
+#include "footprint.hpp"
+#include "types.hpp"
+
+#include <motion_utils/trajectory/trajectory.hpp>
+
+#include <optional>
+#include <vector>
+
+namespace behavior_velocity_planner::out_of_lane
+{
+
+bool can_decelerate(
+  const EgoData & ego_data, const PathPointWithLaneId & point, const double target_vel)
+{
+  if (ego_data.velocity < 0.5) return true;
+  const auto dist_ahead_of_ego = motion_utils::calcSignedArcLength(
+    ego_data.path.points, ego_data.pose.position, point.point.pose.position);
+  const auto acc_to_target_vel =
+    (ego_data.velocity * ego_data.velocity - target_vel * target_vel) / (2 * dist_ahead_of_ego);
+  return acc_to_target_vel < std::abs(ego_data.max_decel);
+}
+
+std::optional<PathPointWithLaneId> calculate_last_in_lane_pose(
+  const EgoData & ego_data, const Slowdown & decision, const Polygon2d & footprint,
+  const PlannerParam & params)
+{
+  const auto from_arc_length =
+    motion_utils::calcSignedArcLength(ego_data.path.points, 0, ego_data.first_path_idx);
+  const auto to_arc_length =
+    motion_utils::calcSignedArcLength(ego_data.path.points, 0, decision.target_path_idx);
+  PathPointWithLaneId interpolated_point;
+  for (auto l = to_arc_length - params.precision; l > from_arc_length; l -= params.precision) {
+    // TODO(Maxime): binary search
+    interpolated_point.point.pose = motion_utils::calcInterpolatedPose(ego_data.path.points, l);
+    const auto respect_decel_limit =
+      !params.skip_if_over_max_decel ||
+      can_decelerate(ego_data, interpolated_point, decision.velocity);
+    if (
+      respect_decel_limit && !boost::geometry::overlaps(
+                               project_to_pose(footprint, interpolated_point.point.pose),
+                               decision.lane_to_avoid.polygon2d().basicPolygon()))
+      return interpolated_point;
+  }
+  return std::nullopt;
+}
+
+/// @brief calculate the slowdown point to insert in the path
+/// @param ego_data ego data (path, velocity, etc)
+/// @param decisions decision (before which point to stop, what lane to avoid entering, etc)
+/// @param params parameters
+/// @return optional slowdown point to insert in the path
+std::optional<SlowdownToInsert> calculate_slowdown_point(
+  const EgoData & ego_data, const std::vector<Slowdown> & decisions, PlannerParam params)
+{
+  params.extra_front_offset += params.dist_buffer;
+  const auto base_footprint = make_base_footprint(params);
+
+  // search for the first slowdown decision for which a stop point can be inserted
+  for (const auto & decision : decisions) {
+    const auto last_in_lane_pose =
+      calculate_last_in_lane_pose(ego_data, decision, base_footprint, params);
+    if (last_in_lane_pose) return SlowdownToInsert{decision, *last_in_lane_pose};
+  }
+  return std::nullopt;
+}
+}  // namespace behavior_velocity_planner::out_of_lane
+#endif  // CALCULATE_SLOWDOWN_POINTS_HPP_

planning/behavior_velocity_out_of_lane_module/src/decisions.cpp

@@ -27,15 +27,15 @@ namespace behavior_velocity_planner::out_of_lane
 double distance_along_path(const EgoData & ego_data, const size_t target_idx)
 {
   return motion_utils::calcSignedArcLength(
-    ego_data.path->points, ego_data.pose.position, ego_data.first_path_idx + target_idx);
+    ego_data.path.points, ego_data.pose.position, ego_data.first_path_idx + target_idx);
 }
 
-double time_along_path(const EgoData & ego_data, const size_t target_idx)
+double time_along_path(const EgoData & ego_data, const size_t target_idx, const double min_velocity)
 {
   const auto dist = distance_along_path(ego_data, target_idx);
   const auto v = std::max(
-    ego_data.velocity,
-    ego_data.path->points[ego_data.first_path_idx + target_idx].point.longitudinal_velocity_mps *
+    std::max(ego_data.velocity, min_velocity),
+    ego_data.path.points[ego_data.first_path_idx + target_idx].point.longitudinal_velocity_mps *
       0.5);
   return dist / v;
 }
@@ -55,8 +55,7 @@ bool object_is_incoming(
 
 std::optional<std::pair<double, double>> object_time_to_range(
   const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range,
-  const std::shared_ptr<route_handler::RouteHandler> route_handler, const double min_confidence,
-  const rclcpp::Logger & logger)
+  const std::shared_ptr<route_handler::RouteHandler> route_handler, const rclcpp::Logger & logger)
 {
   // skip the dynamic object if it is not in a lane preceding the overlapped lane
   // lane changes are intentionally not considered
@@ -65,13 +64,12 @@ std::optional<std::pair<double, double>> object_time_to_range(
     object.kinematics.initial_pose_with_covariance.pose.position.y);
   if (!object_is_incoming(object_point, route_handler, range.lane)) return {};
 
-  const auto max_deviation = object.shape.dimensions.y * 2.0;
+  const auto max_deviation = object.shape.dimensions.y + range.inside_distance;
 
   auto worst_enter_time = std::optional<double>();
   auto worst_exit_time = std::optional<double>();
 
   for (const auto & predicted_path : object.kinematics.predicted_paths) {
-    if (predicted_path.confidence < min_confidence) continue;
     const auto time_step = rclcpp::Duration(predicted_path.time_step).seconds();
     const auto enter_point =
       geometry_msgs::msg::Point().set__x(range.entering_point.x()).set__y(range.entering_point.y());
@@ -119,11 +117,9 @@ std::optional<std::pair<double, double>> object_time_to_range(
 
     const auto same_driving_direction_as_ego = enter_time < exit_time;
     if (same_driving_direction_as_ego) {
-      RCLCPP_DEBUG(logger, " / SAME DIR \\\n");
       worst_enter_time = worst_enter_time ? std::min(*worst_enter_time, enter_time) : enter_time;
       worst_exit_time = worst_exit_time ? std::max(*worst_exit_time, exit_time) : exit_time;
     } else {
-      RCLCPP_DEBUG(logger, " / OPPOSITE DIR \\\n");
       worst_enter_time = worst_enter_time ? std::max(*worst_enter_time, enter_time) : enter_time;
       worst_exit_time = worst_exit_time ? std::min(*worst_exit_time, exit_time) : exit_time;
     }
@@ -205,8 +201,11 @@ std::optional<std::pair<double, double>> object_time_to_range(
 
 bool threshold_condition(const RangeTimes & range_times, const PlannerParam & params)
 {
-  return std::min(range_times.object.enter_time, range_times.object.exit_time) <
-         params.time_threshold;
+  const auto enter_within_threshold =
+    range_times.object.enter_time > 0.0 && range_times.object.enter_time < params.time_threshold;
+  const auto exit_within_threshold =
+    range_times.object.exit_time > 0.0 && range_times.object.exit_time < params.time_threshold;
+  return enter_within_threshold || exit_within_threshold;
 }
 
 bool intervals_condition(
@@ -281,8 +280,10 @@ bool should_not_enter(
   const rclcpp::Logger & logger)
 {
   RangeTimes range_times{};
-  range_times.ego.enter_time = time_along_path(inputs.ego_data, range.entering_path_idx);
-  range_times.ego.exit_time = time_along_path(inputs.ego_data, range.exiting_path_idx);
+  range_times.ego.enter_time =
+    time_along_path(inputs.ego_data, range.entering_path_idx, params.ego_min_velocity);
+  range_times.ego.exit_time =
+    time_along_path(inputs.ego_data, range.exiting_path_idx, params.ego_min_velocity);
   RCLCPP_DEBUG(
     logger, "\t[%lu -> %lu] %ld | ego enters at %2.2f, exits at %2.2f\n", range.entering_path_idx,
     range.exiting_path_idx, range.lane.id(), range_times.ego.enter_time, range_times.ego.exit_time);
@@ -298,8 +299,7 @@ bool should_not_enter(
     // skip objects that are already on the interval
     const auto enter_exit_time =
       params.objects_use_predicted_paths
-        ? object_time_to_range(
-            object, range, inputs.route_handler, params.objects_min_confidence, logger)
+        ? object_time_to_range(object, range, inputs.route_handler, logger)
         : object_time_to_range(object, range, inputs, logger);
     if (!enter_exit_time) {
       RCLCPP_DEBUG(logger, " SKIP (no enter/exit times found)\n");
@@ -349,11 +349,10 @@ std::optional<Slowdown> calculate_decision(
 {
   std::optional<Slowdown> decision;
   if (should_not_enter(range, inputs, params, logger)) {
-    const auto stop_before_range = params.strict ? find_most_preceding_range(range, inputs) : range;
     decision.emplace();
-    decision->target_path_idx = inputs.ego_data.first_path_idx +
-                                stop_before_range.entering_path_idx;  // add offset from curr pose
-    decision->lane_to_avoid = stop_before_range.lane;
+    decision->target_path_idx =
+      inputs.ego_data.first_path_idx + range.entering_path_idx;  // add offset from curr pose
+    decision->lane_to_avoid = range.lane;
     const auto ego_dist_to_range = distance_along_path(inputs.ego_data, range.entering_path_idx);
     set_decision_velocity(decision, ego_dist_to_range, params);
   }

planning/behavior_velocity_out_of_lane_module/src/decisions.hpp

@@ -48,6 +48,7 @@ double distance_along_path(const EgoData & ego_data, const size_t target_idx);
 /// @brief estimate the time when ego will reach some target path index
 /// @param [in] ego_data data related to the ego vehicle
 /// @param [in] target_idx target ego path index
+/// @param [in] min_velocity minimum ego velocity used to estimate the time
 /// @return time taken by ego to reach the target [s]
 double time_along_path(const EgoData & ego_data, const size_t target_idx);
 /// @brief use an object's predicted paths to estimate the times it will reach the enter and exit
@@ -57,14 +58,12 @@ double time_along_path(const EgoData & ego_data, const size_t target_idx);
 /// @param [in] object dynamic object
 /// @param [in] range overlapping range
 /// @param [in] route_handler route handler used to estimate the path of the dynamic object
-/// @param [in] min_confidence minimum confidence to consider a predicted path
 /// @param [in] logger ros logger
 /// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in
 /// the opposite direction, time at enter > time at exit
 std::optional<std::pair<double, double>> object_time_to_range(
   const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range,
-  const std::shared_ptr<route_handler::RouteHandler> route_handler, const double min_confidence,
-  const rclcpp::Logger & logger);
+  const std::shared_ptr<route_handler::RouteHandler> route_handler, const rclcpp::Logger & logger);
 /// @brief use the lanelet map to estimate the times when an object will reach the enter and exit
 /// points of an overlapping range
 /// @param [in] object dynamic object

planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp

@@ -0,0 +1,68 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef FILTER_PREDICTED_OBJECTS_HPP_
+#define FILTER_PREDICTED_OBJECTS_HPP_
+
+#include "types.hpp"
+
+#include <string>
+
+namespace behavior_velocity_planner::out_of_lane
+{
+/// @brief filter predicted objects and their predicted paths
+/// @param [in] objects predicted objects to filter
+/// @param [in] ego_data ego data
+/// @param [in] params parameters
+/// @return filtered predicted objects
+autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects(
+  const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data,
+  const PlannerParam & params)
+{
+  autoware_auto_perception_msgs::msg::PredictedObjects filtered_objects;
+  filtered_objects.header = objects.header;
+  for (const auto & object : objects.objects) {
+    const auto is_pedestrian =
+      std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) {
+        return c.label == autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN;
+      }) != object.classification.end();
+    if (is_pedestrian) continue;
+
+    auto filtered_object = object;
+    const auto is_invalid_predicted_path = [&](const auto & predicted_path) {
+      const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence;
+      const auto lat_offset_to_current_ego =
+        std::abs(motion_utils::calcLateralOffset(predicted_path.path, ego_data.pose.position));
+      const auto is_crossing_ego =
+        lat_offset_to_current_ego <=
+        object.shape.dimensions.y / 2.0 + std::max(
+                                            params.left_offset + params.extra_left_offset,
+                                            params.right_offset + params.extra_right_offset);
+      return is_low_confidence || is_crossing_ego;
+    };
+    if (params.objects_use_predicted_paths) {
+      auto & predicted_paths = filtered_object.kinematics.predicted_paths;
+      const auto new_end =
+        std::remove_if(predicted_paths.begin(), predicted_paths.end(), is_invalid_predicted_path);
+      predicted_paths.erase(new_end, predicted_paths.end());
+    }
+    if (!params.objects_use_predicted_paths || !filtered_object.kinematics.predicted_paths.empty())
+      filtered_objects.objects.push_back(filtered_object);
+  }
+  return filtered_objects;
+}
+
+}  // namespace behavior_velocity_planner::out_of_lane
+
+#endif  // FILTER_PREDICTED_OBJECTS_HPP_

planning/behavior_velocity_out_of_lane_module/src/footprint.cpp

@@ -58,9 +58,9 @@ std::vector<lanelet::BasicPolygon2d> calculate_path_footprints(
 {
   const auto base_footprint = make_base_footprint(params);
   std::vector<lanelet::BasicPolygon2d> path_footprints;
-  path_footprints.reserve(ego_data.path->points.size());
-  for (auto i = ego_data.first_path_idx; i < ego_data.path->points.size(); ++i) {
-    const auto & path_pose = ego_data.path->points[i].point.pose;
+  path_footprints.reserve(ego_data.path.points.size());
+  for (auto i = ego_data.first_path_idx; i < ego_data.path.points.size(); ++i) {
+    const auto & path_pose = ego_data.path.points[i].point.pose;
     const auto angle = tf2::getYaw(path_pose.orientation);
     const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle);
     lanelet::BasicPolygon2d footprint;