feat(out_of_lane): cut predicted paths after red lights (autowarefoun…

…dation#6478)

Signed-off-by: Maxime CLEMENT <[email protected]>

planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml

@@ -18,6 +18,7 @@
                                    # 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
+        cut_predicted_paths_beyond_red_lights: true # if true, predicted paths are cut beyond the stop line of red traffic lights
 
       overlap:
         minimum_distance: 0.0  # [m] minimum distance inside a lanelet for an overlap to be considered

planning/behavior_velocity_out_of_lane_module/package.xml

@@ -27,6 +27,7 @@
   <depend>tf2</depend>
   <depend>tier4_autoware_utils</depend>
   <depend>tier4_planning_msgs</depend>
+  <depend>traffic_light_utils</depend>
   <depend>vehicle_info_util</depend>
   <depend>visualization_msgs</depend>
 

planning/behavior_velocity_out_of_lane_module/src/decisions.cpp

@@ -73,11 +73,13 @@ std::optional<std::pair<double, double>> object_time_to_range(
   if (!object_is_incoming(object_point, route_handler, range.lane)) return {};
 
   const auto max_deviation = object.shape.dimensions.y + range.inside_distance + dist_buffer;
+  const auto max_lon_deviation = object.shape.dimensions.x / 2.0;
   auto worst_enter_time = std::optional<double>();
   auto worst_exit_time = std::optional<double>();
 
   for (const auto & predicted_path : object.kinematics.predicted_paths) {
     const auto unique_path_points = motion_utils::removeOverlapPoints(predicted_path.path);
+    if (unique_path_points.size() < 2) 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());
@@ -121,7 +123,17 @@ std::optional<std::pair<double, double>> object_time_to_range(
         max_deviation);
       continue;
     }
-    // else we rely on the interpolation to estimate beyond the end of the predicted path
+    const auto is_last_predicted_path_point =
+      (exit_segment_idx + 2 == unique_path_points.size() ||
+       enter_segment_idx + 2 == unique_path_points.size());
+    const auto does_not_reach_overlap =
+      is_last_predicted_path_point && (std::min(exit_offset, enter_offset) > max_lon_deviation);
+    if (does_not_reach_overlap) {
+      RCLCPP_DEBUG(
+        logger, " * does not reach the overlap = %2.2fm | max_dev = %2.2fm\n",
+        std::min(exit_offset, enter_offset), max_lon_deviation);
+      continue;
+    }
 
     const auto same_driving_direction_as_ego = enter_time < exit_time;
     if (same_driving_direction_as_ego) {

planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp

@@ -0,0 +1,139 @@
+// Copyright 2023-2024 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.
+
+#include "filter_predicted_objects.hpp"
+
+#include <motion_utils/trajectory/trajectory.hpp>
+#include <traffic_light_utils/traffic_light_utils.hpp>
+
+#include <boost/geometry/algorithms/intersects.hpp>
+
+#include <lanelet2_core/geometry/LaneletMap.h>
+#include <lanelet2_core/primitives/BasicRegulatoryElements.h>
+
+#include <algorithm>
+
+namespace behavior_velocity_planner::out_of_lane
+{
+void cut_predicted_path_beyond_line(
+  autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
+  const lanelet::BasicLineString2d & stop_line, const double object_front_overhang)
+{
+  auto stop_line_idx = 0UL;
+  bool found = false;
+  lanelet::BasicSegment2d path_segment;
+  path_segment.first.x() = predicted_path.path.front().position.x;
+  path_segment.first.y() = predicted_path.path.front().position.y;
+  for (stop_line_idx = 1; stop_line_idx < predicted_path.path.size(); ++stop_line_idx) {
+    path_segment.second.x() = predicted_path.path[stop_line_idx].position.x;
+    path_segment.second.y() = predicted_path.path[stop_line_idx].position.y;
+    if (boost::geometry::intersects(stop_line, path_segment)) {
+      found = true;
+      break;
+    }
+    path_segment.first = path_segment.second;
+  }
+  if (found) {
+    auto cut_idx = stop_line_idx;
+    double arc_length = 0;
+    while (cut_idx > 0 && arc_length < object_front_overhang) {
+      arc_length += tier4_autoware_utils::calcDistance2d(
+        predicted_path.path[cut_idx], predicted_path.path[cut_idx - 1]);
+      --cut_idx;
+    }
+    predicted_path.path.resize(cut_idx);
+  }
+}
+
+std::optional<const lanelet::BasicLineString2d> find_next_stop_line(
+  const autoware_auto_perception_msgs::msg::PredictedPath & path, const PlannerData & planner_data)
+{
+  lanelet::ConstLanelets lanelets;
+  lanelet::BasicLineString2d query_line;
+  for (const auto & p : path.path) query_line.emplace_back(p.position.x, p.position.y);
+  const auto query_results = lanelet::geometry::findWithin2d(
+    planner_data.route_handler_->getLaneletMapPtr()->laneletLayer, query_line);
+  for (const auto & r : query_results) lanelets.push_back(r.second);
+  for (const auto & ll : lanelets) {
+    for (const auto & element : ll.regulatoryElementsAs<lanelet::TrafficLight>()) {
+      const auto traffic_signal_stamped = planner_data.getTrafficSignal(element->id());
+      if (
+        traffic_signal_stamped.has_value() && element->stopLine().has_value() &&
+        traffic_light_utils::isTrafficSignalStop(ll, traffic_signal_stamped.value().signal)) {
+        lanelet::BasicLineString2d stop_line;
+        for (const auto & p : *(element->stopLine())) stop_line.emplace_back(p.x(), p.y());
+        return stop_line;
+      }
+    }
+  }
+  return std::nullopt;
+}
+
+void cut_predicted_path_beyond_red_lights(
+  autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
+  const PlannerData & planner_data, const double object_front_overhang)
+{
+  const auto stop_line = find_next_stop_line(predicted_path, planner_data);
+  if (stop_line) cut_predicted_path_beyond_line(predicted_path, *stop_line, object_front_overhang);
+}
+
+autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects(
+  const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params)
+{
+  autoware_auto_perception_msgs::msg::PredictedObjects filtered_objects;
+  filtered_objects.header = planner_data.predicted_objects->header;
+  for (const auto & object : planner_data.predicted_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 no_overlap_path = motion_utils::removeOverlapPoints(predicted_path.path);
+      if (no_overlap_path.size() <= 1) return true;
+      const auto lat_offset_to_current_ego =
+        std::abs(motion_utils::calcLateralOffset(no_overlap_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_cut_predicted_paths_beyond_red_lights)
+        for (auto & predicted_path : predicted_paths)
+          cut_predicted_path_beyond_red_lights(
+            predicted_path, planner_data, filtered_object.shape.dimensions.x);
+      predicted_paths.erase(
+        std::remove_if(
+          predicted_paths.begin(), predicted_paths.end(),
+          [](const auto & p) { return p.path.empty(); }),
+        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

planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp

@@ -1,4 +1,4 @@
-// Copyright 2023 TIER IV, Inc.
+// Copyright 2023-2024 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.
@@ -17,56 +17,41 @@
 
 #include "types.hpp"
 
-#include <motion_utils/trajectory/trajectory.hpp>
+#include <behavior_velocity_planner_common/planner_data.hpp>
 
-#include <string>
+#include <optional>
 
 namespace behavior_velocity_planner::out_of_lane
 {
+/// @brief cut a predicted path beyond the given stop line
+/// @param [inout] predicted_path predicted path to cut
+/// @param [in] stop_line stop line used for cutting
+/// @param [in] object_front_overhang extra distance to cut ahead of the stop line
+void cut_predicted_path_beyond_line(
+  autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
+  const lanelet::BasicLineString2d & stop_line, const double object_front_overhang);
+
+/// @brief find the next red light stop line along the given path
+/// @param [in] path predicted path to check for a stop line
+/// @param [in] planner_data planner data with stop line information
+/// @return the first red light stop line found along the path (if any)
+std::optional<const lanelet::BasicLineString2d> find_next_stop_line(
+  const autoware_auto_perception_msgs::msg::PredictedPath & path, const PlannerData & planner_data);
+
+/// @brief cut predicted path beyond stop lines of red lights
+/// @param [inout] predicted_path predicted path to cut
+/// @param [in] planner_data planner data to get the map and traffic light information
+void cut_predicted_path_beyond_red_lights(
+  autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
+  const PlannerData & planner_data, const double object_front_overhang);
+
 /// @brief filter predicted objects and their predicted paths
-/// @param [in] objects predicted objects to filter
+/// @param [in] planner_data planner data
 /// @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 no_overlap_path = motion_utils::removeOverlapPoints(predicted_path.path);
-      if (no_overlap_path.size() <= 1) return true;
-      const auto lat_offset_to_current_ego =
-        std::abs(motion_utils::calcLateralOffset(no_overlap_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;
-}
-
+  const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params);
 }  // namespace behavior_velocity_planner::out_of_lane
 
 #endif  // FILTER_PREDICTED_OBJECTS_HPP_

planning/behavior_velocity_out_of_lane_module/src/manager.cpp

@@ -51,6 +51,8 @@ OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node)
   pp.objects_min_confidence =
     getOrDeclareParameter<double>(node, ns + ".objects.predicted_path_min_confidence");
   pp.objects_dist_buffer = getOrDeclareParameter<double>(node, ns + ".objects.distance_buffer");
+  pp.objects_cut_predicted_paths_beyond_red_lights =
+    getOrDeclareParameter<bool>(node, ns + ".objects.cut_predicted_paths_beyond_red_lights");
 
   pp.overlap_min_dist = getOrDeclareParameter<double>(node, ns + ".overlap.minimum_distance");
   pp.overlap_extra_length = getOrDeclareParameter<double>(node, ns + ".overlap.extra_length");

planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp

@@ -109,13 +109,15 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
     calculate_overlapping_ranges(path_footprints, path_lanelets, other_lanelets, params_);
   const auto calculate_overlapping_ranges_us = stopwatch.toc("calculate_overlapping_ranges");
   // Calculate stop and slowdown points
-  stopwatch.tic("calculate_decisions");
   DecisionInputs inputs;
   inputs.ranges = ranges;
   inputs.ego_data = ego_data;
-  inputs.objects = filter_predicted_objects(*planner_data_->predicted_objects, ego_data, params_);
+  stopwatch.tic("filter_predicted_objects");
+  inputs.objects = filter_predicted_objects(*planner_data_, ego_data, params_);
+  const auto filter_predicted_objects_ms = stopwatch.toc("filter_predicted_objects");
   inputs.route_handler = planner_data_->route_handler_;
   inputs.lanelets = other_lanelets;
+  stopwatch.tic("calculate_decisions");
   const auto decisions = calculate_decisions(inputs, params_, logger_);
   const auto calculate_decisions_us = stopwatch.toc("calculate_decisions");
   stopwatch.tic("calc_slowdown_points");
@@ -157,7 +159,7 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
   }
   if (point_to_insert) {
     prev_inserted_point_ = point_to_insert;
-    RCLCPP_INFO(logger_, "Avoiding lane %lu", point_to_insert->slowdown.lane_to_avoid.id());
+    RCLCPP_DEBUG(logger_, "Avoiding lane %lu", point_to_insert->slowdown.lane_to_avoid.id());
     debug_data_.slowdowns = {*point_to_insert};
     auto path_idx = motion_utils::findNearestSegmentIndex(
                       path->points, point_to_insert->point.point.pose.position) +
@@ -187,12 +189,13 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
     "\tcalculate_lanelets = %2.0fus\n"
     "\tcalculate_path_footprints = %2.0fus\n"
     "\tcalculate_overlapping_ranges = %2.0fus\n"
+    "\tfilter_pred_objects = %2.0fus\n"
     "\tcalculate_decisions = %2.0fus\n"
     "\tcalc_slowdown_points = %2.0fus\n"
     "\tinsert_slowdown_points = %2.0fus\n",
     total_time_us, calculate_lanelets_us, calculate_path_footprints_us,
-    calculate_overlapping_ranges_us, calculate_decisions_us, calc_slowdown_points_us,
-    insert_slowdown_points_us);
+    calculate_overlapping_ranges_us, filter_predicted_objects_ms, calculate_decisions_us,
+    calc_slowdown_points_us, insert_slowdown_points_us);
   return true;
 }
 

planning/behavior_velocity_out_of_lane_module/src/types.hpp

@@ -47,8 +47,10 @@ struct PlannerParam
   double ego_min_velocity;  // [m/s] minimum velocity of ego used to calculate its ttc or time range
 
   bool objects_use_predicted_paths;  // whether to use the objects' predicted paths
-  double objects_min_vel;            // [m/s] objects lower than this velocity will be ignored
-  double objects_min_confidence;     // minimum confidence to consider a predicted path
+  bool objects_cut_predicted_paths_beyond_red_lights;  // whether to cut predicted paths beyond red
+                                                       // lights' stop lines
+  double objects_min_vel;         // [m/s] objects lower than this velocity will be ignored
+  double objects_min_confidence;  // minimum confidence to consider a predicted path
   double objects_dist_buffer;  // [m] distance buffer used to determine if a collision will occur in
                                // the other lane