feat(out_of_lane): more stable decisions

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml

@@ -17,6 +17,7 @@
         use_predicted_paths: true  # if true, use the predicted paths to estimate future positions.
                                    # if false, assume the object moves at constant velocity along *all* lanelets it currently is located in.
         predicted_path_min_confidence : 0.1  # when using predicted paths, ignore the ones whose confidence is lower than this value.
+        distance_buffer: 1.0 # [m] distance buffer used to determine if a collision will occur in the other lane
 
       overlap:
         minimum_distance: 0.0  # [m] minimum distance inside a lanelet for an overlap to be considered
@@ -27,6 +28,7 @@
         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.
         distance_buffer: 1.5  # [m] buffer distance to try to keep between the ego footprint and lane
+        min_duration: 1.0  # [s] minimum duration needed before a decision can be canceled
         slowdown:
           distance_threshold: 30.0 # [m] insert a slowdown when closer than this distance from an overlap
           velocity: 2.0  # [m/s] slowdown velocity

planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp

@@ -0,0 +1,103 @@
+// 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
+{
+
+/// @brief estimate whether ego can decelerate without breaking the deceleration limit
+/// @details assume ego wants to reach the target point at the target velocity
+/// @param [in] ego_data ego data
+/// @param [in] point target point
+/// @param [in] target_vel target_velocity
+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);
+}
+
+/// @brief calculate the last pose along the path where ego does not overlap the lane to avoid
+/// @param [in] ego_data ego data
+/// @param [in] decision the input decision (i.e., which lane to avoid and at what speed)
+/// @param [in] footprint the ego footprint
+/// @param [in] prev_slowdown_point previous slowdown point. If set, ignore deceleration limits
+/// @param [in] params parameters
+/// @return the last pose that is not out of lane (if found)
+std::optional<PathPointWithLaneId> calculate_last_in_lane_pose(
+  const EgoData & ego_data, const Slowdown & decision, const Polygon2d & footprint,
+  const std::optional<SlowdownToInsert> & prev_slowdown_point, 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 || prev_slowdown_point ||
+      can_decelerate(ego_data, interpolated_point, decision.velocity);
+    const auto interpolated_footprint = project_to_pose(footprint, interpolated_point.point.pose);
+    const auto is_overlap_lane = boost::geometry::overlaps(
+      interpolated_footprint, decision.lane_to_avoid.polygon2d().basicPolygon());
+    const auto is_overlap_extra_lane =
+      prev_slowdown_point &&
+      boost::geometry::overlaps(
+        interpolated_footprint,
+        prev_slowdown_point->slowdown.lane_to_avoid.polygon2d().basicPolygon());
+    if (respect_decel_limit && !is_overlap_lane && !is_overlap_extra_lane)
+      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 prev_slowdown_point previously calculated slowdown point
+/// @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,
+  const std::optional<SlowdownToInsert> prev_slowdown_point, 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, prev_slowdown_point, 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/debug.cpp

@@ -34,13 +34,12 @@ visualization_msgs::msg::Marker get_base_marker()
   base_marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0);
   base_marker.scale = tier4_autoware_utils::createMarkerScale(0.1, 0.1, 0.1);
   base_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5);
-  base_marker.lifetime = rclcpp::Duration::from_seconds(0.5);
   return base_marker;
 }
 }  // namespace
 void add_footprint_markers(
   visualization_msgs::msg::MarkerArray & debug_marker_array,
-  const lanelet::BasicPolygons2d & footprints, const double z)
+  const lanelet::BasicPolygons2d & footprints, const double z, const size_t prev_nb)
 {
   auto debug_marker = get_base_marker();
   debug_marker.ns = "footprints";
@@ -54,12 +53,14 @@ void add_footprint_markers(
     debug_marker.id++;
     debug_marker.points.clear();
   }
+  for (; debug_marker.id < static_cast<int>(prev_nb); ++debug_marker.id)
+    debug_marker_array.markers.push_back(debug_marker);
 }
 
 void add_current_overlap_marker(
   visualization_msgs::msg::MarkerArray & debug_marker_array,
   const lanelet::BasicPolygon2d & current_footprint,
-  const lanelet::ConstLanelets & current_overlapped_lanelets, const double z)
+  const lanelet::ConstLanelets & current_overlapped_lanelets, const double z, const size_t prev_nb)
 {
   auto debug_marker = get_base_marker();
   debug_marker.ns = "current_overlap";
@@ -82,12 +83,14 @@ void add_current_overlap_marker(
     debug_marker_array.markers.push_back(debug_marker);
     debug_marker.id++;
   }
+  for (; debug_marker.id < static_cast<int>(prev_nb); ++debug_marker.id)
+    debug_marker_array.markers.push_back(debug_marker);
 }
 
 void add_lanelet_markers(
   visualization_msgs::msg::MarkerArray & debug_marker_array,
   const lanelet::ConstLanelets & lanelets, const std::string & ns,
-  const std_msgs::msg::ColorRGBA & color)
+  const std_msgs::msg::ColorRGBA & color, const size_t prev_nb)
 {
   auto debug_marker = get_base_marker();
   debug_marker.ns = ns;
@@ -103,6 +106,82 @@ void add_lanelet_markers(
     debug_marker_array.markers.push_back(debug_marker);
     debug_marker.id++;
   }
+  debug_marker.action = debug_marker.DELETE;
+  for (; debug_marker.id < static_cast<int>(prev_nb); ++debug_marker.id)
+    debug_marker_array.markers.push_back(debug_marker);
+}
+
+void add_range_markers(
+  visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges,
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t first_ego_idx,
+  const double z, const size_t prev_nb)
+{
+  auto debug_marker = get_base_marker();
+  debug_marker.ns = "ranges";
+  debug_marker.color = tier4_autoware_utils::createMarkerColor(0.2, 0.9, 0.1, 0.5);
+  for (const auto & range : ranges) {
+    debug_marker.points.clear();
+    debug_marker.points.push_back(
+      path.points[first_ego_idx + range.entering_path_idx].point.pose.position);
+    debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition(
+      range.entering_point.x(), range.entering_point.y(), z));
+    for (const auto & overlap : range.debug.overlaps) {
+      debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition(
+        overlap.min_overlap_point.x(), overlap.min_overlap_point.y(), z));
+      debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition(
+        overlap.max_overlap_point.x(), overlap.max_overlap_point.y(), z));
+    }
+    debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition(
+      range.exiting_point.x(), range.exiting_point.y(), z));
+    debug_marker.points.push_back(
+      path.points[first_ego_idx + range.exiting_path_idx].point.pose.position);
+    debug_marker_array.markers.push_back(debug_marker);
+    debug_marker.id++;
+  }
+  debug_marker.action = debug_marker.DELETE;
+  for (; debug_marker.id < static_cast<int>(prev_nb); ++debug_marker.id)
+    debug_marker_array.markers.push_back(debug_marker);
+  debug_marker.action = debug_marker.ADD;
+  debug_marker.id = 0;
+  debug_marker.ns = "decisions";
+  debug_marker.color = tier4_autoware_utils::createMarkerColor(0.9, 0.1, 0.1, 1.0);
+  debug_marker.points.clear();
+  for (const auto & range : ranges) {
+    debug_marker.type = debug_marker.LINE_STRIP;
+    if (range.debug.decision) {
+      debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition(
+        range.entering_point.x(), range.entering_point.y(), z));
+      debug_marker.points.push_back(
+        range.debug.object->kinematics.initial_pose_with_covariance.pose.position);
+    }
+    debug_marker_array.markers.push_back(debug_marker);
+    debug_marker.points.clear();
+    debug_marker.id++;
+
+    debug_marker.type = debug_marker.TEXT_VIEW_FACING;
+    debug_marker.pose.position.x = range.entering_point.x();
+    debug_marker.pose.position.y = range.entering_point.y();
+    debug_marker.pose.position.z = z;
+    std::stringstream ss;
+    ss << "Ego: " << range.debug.times.ego.enter_time << " - " << range.debug.times.ego.exit_time
+       << "\n";
+    if (range.debug.object) {
+      debug_marker.pose.position.x +=
+        range.debug.object->kinematics.initial_pose_with_covariance.pose.position.x;
+      debug_marker.pose.position.y +=
+        range.debug.object->kinematics.initial_pose_with_covariance.pose.position.y;
+      debug_marker.pose.position.x /= 2;
+      debug_marker.pose.position.y /= 2;
+      ss << "Obj: " << range.debug.times.object.enter_time << " - "
+         << range.debug.times.object.exit_time << "\n";
+    }
+    debug_marker.scale.z = 1.0;
+    debug_marker.text = ss.str();
+    debug_marker_array.markers.push_back(debug_marker);
+    debug_marker.id++;
+  }
+  for (; debug_marker.id < static_cast<int>(prev_nb); ++debug_marker.id)
+    debug_marker_array.markers.push_back(debug_marker);
 }
 
 }  // namespace behavior_velocity_planner::out_of_lane::debug

planning/behavior_velocity_out_of_lane_module/src/debug.hpp

@@ -29,27 +29,41 @@ namespace behavior_velocity_planner::out_of_lane::debug
 /// @param [inout] debug_marker_array marker array
 /// @param [in] footprints footprints to turn into markers
 /// @param [in] z z value to use for the markers
+/// @param [in] prev_nb previous number of markers (used to delete the extra ones)
 void add_footprint_markers(
   visualization_msgs::msg::MarkerArray & debug_marker_array,
-  const lanelet::BasicPolygons2d & footprints, const double z);
+  const lanelet::BasicPolygons2d & footprints, const double z, const size_t prev_nb);
 /// @brief add footprint markers to the given marker array
 /// @param [inout] debug_marker_array marker array
 /// @param [in] current_footprint footprint to turn into a marker
 /// @param [in] current_overlapped_lanelets lanelets to turn into markers
 /// @param [in] z z value to use for the markers
+/// @param [in] prev_nb previous number of markers (used to delete the extra ones)
 void add_current_overlap_marker(
   visualization_msgs::msg::MarkerArray & debug_marker_array,
   const lanelet::BasicPolygon2d & current_footprint,
-  const lanelet::ConstLanelets & current_overlapped_lanelets, const double z);
+  const lanelet::ConstLanelets & current_overlapped_lanelets, const double z, const size_t prev_nb);
 /// @brief add footprint markers to the given marker array
 /// @param [inout] debug_marker_array marker array
 /// @param [in] lanelets lanelets to turn into markers
 /// @param [in] ns namespace of the markers
 /// @param [in] color namespace of the markers
+/// @param [in] prev_nb previous number of markers (used to delete the extra ones)
 void add_lanelet_markers(
   visualization_msgs::msg::MarkerArray & debug_marker_array,
   const lanelet::ConstLanelets & lanelets, const std::string & ns,
-  const std_msgs::msg::ColorRGBA & color);
+  const std_msgs::msg::ColorRGBA & color, const size_t prev_nb);
+/// @brief add ranges markers to the given marker array
+/// @param [inout] debug_marker_array marker array
+/// @param [in] ranges ranges to turn into markers
+/// @param [in] path modified ego path that was used to calculate the ranges
+/// @param [in] first_path_idx first idx of ego on the path
+/// @param [in] z z value to use for the markers
+/// @param [in] prev_nb previous number of markers (used to delete the extra ones)
+void add_range_markers(
+  visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges,
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t first_path_idx,
+  const double z, const size_t prev_nb);
 }  // namespace behavior_velocity_planner::out_of_lane::debug
 
 #endif  // DEBUG_HPP_

planning/behavior_velocity_out_of_lane_module/src/decisions.cpp

@@ -26,7 +26,6 @@
 #include <lanelet2_routing/RoutingGraph.h>
 
 #include <algorithm>
-#include <limits>
 #include <memory>
 #include <utility>
 #include <vector>
@@ -63,7 +62,8 @@ 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 rclcpp::Logger & logger)
+  const std::shared_ptr<route_handler::RouteHandler> route_handler, const double dist_buffer,
+  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
@@ -72,8 +72,7 @@ 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 + range.inside_distance;
-
+  const auto max_deviation = object.shape.dimensions.y + range.inside_distance + dist_buffer;
   auto worst_enter_time = std::optional<double>();
   auto worst_exit_time = std::optional<double>();
 
@@ -307,7 +306,8 @@ 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, logger)
+        ? object_time_to_range(
+            object, range, inputs.route_handler, params.objects_dist_buffer, logger)
         : object_time_to_range(object, range, inputs, logger);
     if (!enter_exit_time) {
       RCLCPP_DEBUG(logger, " SKIP (no enter/exit times found)\n");
@@ -316,27 +316,14 @@ bool should_not_enter(
 
     range_times.object.enter_time = enter_exit_time->first;
     range_times.object.exit_time = enter_exit_time->second;
-    if (will_collide_on_range(range_times, params, logger)) return true;
-  }
-  return false;
-}
-
-OverlapRange find_most_preceding_range(const OverlapRange & range, const DecisionInputs & inputs)
-{
-  OverlapRange preceding_range = range;
-  bool found_preceding_range = true;
-  while (found_preceding_range) {
-    found_preceding_range = false;
-    for (const auto & other_range : inputs.ranges) {
-      if (
-        other_range.entering_path_idx < preceding_range.entering_path_idx &&
-        other_range.exiting_path_idx >= preceding_range.entering_path_idx) {
-        preceding_range = other_range;
-        found_preceding_range = true;
-      }
+    if (will_collide_on_range(range_times, params, logger)) {
+      range.debug.times = range_times;
+      range.debug.object = object;
+      return true;
     }
   }
-  return preceding_range;
+  range.debug.times = range_times;
+  return false;
 }
 
 void set_decision_velocity(
@@ -375,6 +362,7 @@ std::vector<Slowdown> calculate_decisions(
   for (const auto & range : inputs.ranges) {
     if (range.entering_path_idx == 0UL) continue;  // skip if we already entered the range
     const auto optional_decision = calculate_decision(range, inputs, params, logger);
+    range.debug.decision = optional_decision;
     if (optional_decision) decisions.push_back(*optional_decision);
   }
   return decisions;