feat(out_of_lane): more stable stop decisions (#896)

* feat(out_of_lane): more stable decisions Signed-off-by: Maxime CLEMENT <[email protected]> * Remove tf2_geometry_msg changes Signed-off-by: Maxime CLEMENT <[email protected]> --------- Signed-off-by: Maxime CLEMENT <[email protected]>
tier4 · Oct 7, 2023 · 7ac2439 · 7ac2439
1 parent 5ddb2e2
commit 7ac2439
Show file tree

Hide file tree

Showing 12 changed files with 273 additions and 91 deletions.
diff --git a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml b/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
@@ -26,6 +27,7 @@
         skip_if_over_max_decel: true  # if true, do not take an action that would cause more deceleration than the maximum allowed
         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
+        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

diff --git a/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp b/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp
@@ -26,6 +26,11 @@
 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)
 {
@@ -37,9 +42,16 @@ bool can_decelerate(
   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 PlannerParam & params)
+  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);
@@ -50,12 +62,17 @@ std::optional<PathPointWithLaneId> calculate_last_in_lane_pose(
     // 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 ||
+      !params.skip_if_over_max_decel || prev_slowdown_point ||
       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()))
+    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;
@@ -64,18 +81,20 @@ std::optional<PathPointWithLaneId> calculate_last_in_lane_pose(
 /// @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, PlannerParam params)
+  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, params);
+      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;

diff --git a/planning/behavior_velocity_out_of_lane_module/src/debug.cpp b/planning/behavior_velocity_out_of_lane_module/src/debug.cpp
@@ -32,13 +32,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";
@@ -52,12 +51,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";
@@ -80,12 +81,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;
@@ -101,6 +104,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
diff --git a/planning/behavior_velocity_out_of_lane_module/src/debug.hpp b/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_
diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp
@@ -17,7 +17,6 @@
 #include <lanelet2_extension/utility/utilities.hpp>
 
 #include <algorithm>
-#include <limits>
 #include <memory>
 #include <utility>
 #include <vector>
@@ -55,7 +54,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
@@ -64,8 +64,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>();
 
@@ -300,7 +299,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");
@@ -309,27 +309,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(
@@ -367,6 +354,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;

diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp
@@ -30,16 +30,6 @@
 
 namespace behavior_velocity_planner::out_of_lane
 {
-struct EnterExitTimes
-{
-  double enter_time;
-  double exit_time;
-};
-struct RangeTimes
-{
-  EnterExitTimes ego;
-  EnterExitTimes object;
-};
 /// @brief calculate the distance along the ego path between ego and some target path index
 /// @param [in] ego_data data related to the ego vehicle
 /// @param [in] target_idx target ego path index
@@ -63,13 +53,15 @@ double time_along_path(const EgoData & ego_data, const size_t target_idx);
 /// 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 rclcpp::Logger & logger);
+  const std::shared_ptr<route_handler::RouteHandler> route_handler, const double dist_buffer,
+  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
 /// @param [in] range overlapping range
 /// @param [in] inputs information used to take decisions (ranges, ego and objects data, route
 /// handler, lanelets)
+/// @param [in] dist_buffer extra distance used to estimate if a collision will occur on the range
 /// @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.
@@ -93,14 +85,6 @@ bool will_collide_on_range(
 bool should_not_enter(
   const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params,
   const rclcpp::Logger & logger);
-/// @brief find the most preceding range
-/// @details the most preceding range is the first range in a succession of ranges with overlapping
-/// enter/exit indexes.
-/// @param [in] range range
-/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route
-/// handler, lanelets)
-/// @return most preceding range
-OverlapRange find_most_preceding_range(const OverlapRange & range, const DecisionInputs & inputs);
 /// @brief set the velocity of a decision (or unset it) based on the distance away from the range
 /// @param [out] decision decision to update (either set its velocity or unset the decision)
 /// @param [in] distance distance between ego and the range corresponding to the decision