fix(out_of_lane): improve stop decision

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

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/s] 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

planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp

@@ -0,0 +1,87 @@
+// 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/interpolation.hpp>
+#include <motion_utils/trajectory/trajectory.hpp>
+
+#include <boost/geometry/algorithms/overlaps.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

@@ -35,15 +35,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, const double min_velocity)
 {
   const auto dist = distance_along_path(ego_data, target_idx);
   const auto v = std::max(
     std::max(ego_data.velocity, min_velocity),
-    ego_data.path->points[ego_data.first_path_idx + target_idx].point.longitudinal_velocity_mps *
+    ego_data.path.points[ego_data.first_path_idx + target_idx].point.longitudinal_velocity_mps *
       0.5);
   return dist / v;
 }
@@ -126,11 +126,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;
     }
@@ -212,8 +210,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(
@@ -357,11 +358,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/filter_predicted_objects.hpp

@@ -23,9 +23,6 @@
 
 namespace behavior_velocity_planner::out_of_lane
 {
-using motion_utils::calcLateralOffset;
-using tier4_autoware_utils::calcDistance2d;
-
 /// @brief filter predicted objects and their predicted paths
 /// @param [in] objects predicted objects to filter
 /// @param [in] ego_data ego data
@@ -47,13 +44,8 @@ autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects(
     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 unique_path_points = motion_utils::removeOverlapPoints(predicted_path.path);
-
-      // Note: lateral offset can not be calculated for path with less than 2 unique points
       const auto lat_offset_to_current_ego =
-        unique_path_points.size() > 1
-          ? std::abs(calcLateralOffset(unique_path_points, ego_data.pose.position))
-          : calcDistance2d(unique_path_points.front(), ego_data.pose.position);
+        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(

planning/behavior_velocity_out_of_lane_module/src/footprint.cpp

@@ -57,9 +57,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;

planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp

@@ -20,6 +20,7 @@
 #include <lanelet2_routing/RoutingGraph.h>
 
 #include <algorithm>
+#include <string>
 
 namespace behavior_velocity_planner::out_of_lane
 {
@@ -68,6 +69,8 @@ lanelet::ConstLanelets calculate_other_lanelets(
     return false;
   };
   for (const auto & ll : lanelets_within_range) {
+    if (std::string(ll.second.attributeOr(lanelet::AttributeName::Subtype, "none")) != "road")
+      continue;
     const auto is_path_lanelet = contains_lanelet(path_lanelets, ll.second.id());
     const auto is_ignored_lanelet = contains_lanelet(ignored_lanelets, ll.second.id());
     if (!is_path_lanelet && !is_ignored_lanelet && !is_overlapped_by_path_lanelets(ll.second))

planning/behavior_velocity_out_of_lane_module/src/manager.cpp

@@ -56,7 +56,7 @@ OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node)
 
   pp.skip_if_over_max_decel =
     getOrDeclareParameter<bool>(node, ns + ".action.skip_if_over_max_decel");
-  pp.strict = getOrDeclareParameter<bool>(node, ns + ".action.strict");
+  pp.precision = getOrDeclareParameter<double>(node, ns + ".action.precision");
   pp.dist_buffer = getOrDeclareParameter<double>(node, ns + ".action.distance_buffer");
   pp.slow_velocity = getOrDeclareParameter<double>(node, ns + ".action.slowdown.velocity");
   pp.slow_dist_threshold =