Merge pull request #1233 from maxime-clem/cherry-pick/out_of_lane

feat(out_of_lane): cherry-pick lane change bug fix + cut pred paths at red light

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,146 @@
+// 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) {
+    // we use a longer stop line to also cut predicted paths that slightly go around the stop line
+    auto longer_stop_line = *stop_line;
+    const auto diff = stop_line->back() - stop_line->front();
+    longer_stop_line.front() -= diff * 0.5;
+    longer_stop_line.back() += diff * 0.5;
+    cut_predicted_path_beyond_line(predicted_path, longer_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/lanelets_selection.cpp

@@ -24,12 +24,47 @@
 
 namespace behavior_velocity_planner::out_of_lane
 {
+
+lanelet::ConstLanelets consecutive_lanelets(
+  const route_handler::RouteHandler & route_handler, const lanelet::ConstLanelet & lanelet)
+{
+  lanelet::ConstLanelets consecutives = route_handler.getRoutingGraphPtr()->following(lanelet);
+  const auto previous = route_handler.getRoutingGraphPtr()->previous(lanelet);
+  consecutives.insert(consecutives.end(), previous.begin(), previous.end());
+  return consecutives;
+}
+
+lanelet::ConstLanelets get_missing_lane_change_lanelets(
+  lanelet::ConstLanelets & path_lanelets, const route_handler::RouteHandler & route_handler)
+{
+  lanelet::ConstLanelets missing_lane_change_lanelets;
+  const auto & routing_graph = *route_handler.getRoutingGraphPtr();
+  lanelet::ConstLanelets adjacents;
+  lanelet::ConstLanelets consecutives;
+  for (const auto & ll : path_lanelets) {
+    const auto consecutives_of_ll = consecutive_lanelets(route_handler, ll);
+    std::copy_if(
+      consecutives_of_ll.begin(), consecutives_of_ll.end(), std::back_inserter(consecutives),
+      [&](const auto & l) { return !contains_lanelet(consecutives, l.id()); });
+    const auto adjacents_of_ll = routing_graph.besides(ll);
+    std::copy_if(
+      adjacents_of_ll.begin(), adjacents_of_ll.end(), std::back_inserter(adjacents),
+      [&](const auto & l) { return !contains_lanelet(adjacents, l.id()); });
+  }
+  std::copy_if(
+    adjacents.begin(), adjacents.end(), std::back_inserter(missing_lane_change_lanelets),
+    [&](const auto & l) {
+      return !contains_lanelet(missing_lane_change_lanelets, l.id()) &&
+             !contains_lanelet(path_lanelets, l.id()) && contains_lanelet(consecutives, l.id());
+    });
+  return missing_lane_change_lanelets;
+}
+
 lanelet::ConstLanelets calculate_path_lanelets(
   const EgoData & ego_data, const route_handler::RouteHandler & route_handler)
 {
   const auto lanelet_map_ptr = route_handler.getLaneletMapPtr();
-  lanelet::ConstLanelets path_lanelets =
-    planning_utils::getLaneletsOnPath(ego_data.path, lanelet_map_ptr, ego_data.pose);
+  lanelet::ConstLanelets path_lanelets;
   lanelet::BasicLineString2d path_ls;
   for (const auto & p : ego_data.path.points)
     path_ls.emplace_back(p.point.pose.position.x, p.point.pose.position.y);
@@ -38,6 +73,8 @@ lanelet::ConstLanelets calculate_path_lanelets(
     if (!contains_lanelet(path_lanelets, dist_lanelet.second.id()))
       path_lanelets.push_back(dist_lanelet.second);
   }
+  const auto missing_lanelets = get_missing_lane_change_lanelets(path_lanelets, route_handler);
+  path_lanelets.insert(path_lanelets.end(), missing_lanelets.begin(), missing_lanelets.end());
   return path_lanelets;
 }
 

planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp

@@ -41,6 +41,13 @@ inline bool contains_lanelet(const lanelet::ConstLanelets & lanelets, const lane
 /// @return lanelets crossed by the ego vehicle
 lanelet::ConstLanelets calculate_path_lanelets(
   const EgoData & ego_data, const route_handler::RouteHandler & route_handler);
+/// @brief calculate lanelets that may not be crossed by the path but may be overlapped during a
+/// lane change
+/// @param [in] path_lanelets lanelets driven by the ego vehicle
+/// @param [in] route_handler route handler
+/// @return lanelets that may be overlapped by a lane change (and are not already in path_lanelets)
+lanelet::ConstLanelets get_missing_lane_change_lanelets(
+  lanelet::ConstLanelets & path_lanelets, const route_handler::RouteHandler & route_handler);
 /// @brief calculate lanelets that should be ignored
 /// @param [in] ego_data data about the ego vehicle
 /// @param [in] path_lanelets lanelets driven by the ego vehicle

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");