feat(out_of_lane): filter predicted paths and add min assumed velocity (

autowarefoundation#4784)

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

planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml

@@ -34,6 +34,7 @@
           distance_threshold: 15.0 # [m] insert a stop when closer than this distance from an overlap
 
       ego:
+        min_assumed_velocity: 2.0 # [m/s] minimum velocity used to calculate the enter and exit times of ego
         extra_front_offset: 0.0 # [m] extra front distance
         extra_rear_offset: 0.0 # [m] extra rear distance
         extra_right_offset: 0.0 # [m] extra right distance

planning/behavior_velocity_out_of_lane_module/src/decisions.cpp

@@ -30,11 +30,11 @@ double distance_along_path(const EgoData & ego_data, const size_t 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)
+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(
-    ego_data.velocity,
+    std::max(ego_data.velocity, min_velocity),
     ego_data.path->points[ego_data.first_path_idx + target_idx].point.longitudinal_velocity_mps *
       0.5);
   return dist / v;
@@ -55,8 +55,7 @@ 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 double min_confidence,
-  const rclcpp::Logger & logger)
+  const std::shared_ptr<route_handler::RouteHandler> route_handler, 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
@@ -65,13 +64,12 @@ 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 * 2.0;
+  const auto max_deviation = object.shape.dimensions.y + range.inside_distance;
 
   auto worst_enter_time = std::optional<double>();
   auto worst_exit_time = std::optional<double>();
 
   for (const auto & predicted_path : object.kinematics.predicted_paths) {
-    if (predicted_path.confidence < min_confidence) 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());
@@ -281,8 +279,10 @@ bool should_not_enter(
   const rclcpp::Logger & logger)
 {
   RangeTimes range_times{};
-  range_times.ego.enter_time = time_along_path(inputs.ego_data, range.entering_path_idx);
-  range_times.ego.exit_time = time_along_path(inputs.ego_data, range.exiting_path_idx);
+  range_times.ego.enter_time =
+    time_along_path(inputs.ego_data, range.entering_path_idx, params.ego_min_velocity);
+  range_times.ego.exit_time =
+    time_along_path(inputs.ego_data, range.exiting_path_idx, params.ego_min_velocity);
   RCLCPP_DEBUG(
     logger, "\t[%lu -> %lu] %ld | ego enters at %2.2f, exits at %2.2f\n", range.entering_path_idx,
     range.exiting_path_idx, range.lane.id(), range_times.ego.enter_time, range_times.ego.exit_time);
@@ -298,8 +298,7 @@ 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, params.objects_min_confidence, logger)
+        ? object_time_to_range(object, range, inputs.route_handler, logger)
         : object_time_to_range(object, range, inputs, logger);
     if (!enter_exit_time) {
       RCLCPP_DEBUG(logger, " SKIP (no enter/exit times found)\n");

planning/behavior_velocity_out_of_lane_module/src/decisions.hpp

@@ -48,6 +48,7 @@ double distance_along_path(const EgoData & ego_data, const size_t target_idx);
 /// @brief estimate the time when ego will reach some target path index
 /// @param [in] ego_data data related to the ego vehicle
 /// @param [in] target_idx target ego path index
+/// @param [in] min_velocity minimum ego velocity used to estimate the time
 /// @return time taken by ego to reach the target [s]
 double time_along_path(const EgoData & ego_data, const size_t target_idx);
 /// @brief use an object's predicted paths to estimate the times it will reach the enter and exit
@@ -57,14 +58,12 @@ double time_along_path(const EgoData & ego_data, const size_t target_idx);
 /// @param [in] object dynamic object
 /// @param [in] range overlapping range
 /// @param [in] route_handler route handler used to estimate the path of the dynamic object
-/// @param [in] min_confidence minimum confidence to consider a predicted path
 /// @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
 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 double min_confidence,
-  const rclcpp::Logger & logger);
+  const std::shared_ptr<route_handler::RouteHandler> route_handler, 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

planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp

@@ -0,0 +1,68 @@
+// 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 FILTER_PREDICTED_OBJECTS_HPP_
+#define FILTER_PREDICTED_OBJECTS_HPP_
+
+#include "types.hpp"
+
+#include <string>
+
+namespace behavior_velocity_planner::out_of_lane
+{
+/// @brief filter predicted objects and their predicted paths
+/// @param [in] objects predicted objects to filter
+/// @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 lat_offset_to_current_ego =
+        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(
+                                            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;
+}
+
+}  // namespace behavior_velocity_planner::out_of_lane
+
+#endif  // FILTER_PREDICTED_OBJECTS_HPP_

planning/behavior_velocity_out_of_lane_module/src/manager.cpp

@@ -58,6 +58,7 @@ OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node)
     node.declare_parameter<double>(ns + ".action.slowdown.distance_threshold");
   pp.stop_dist_threshold = node.declare_parameter<double>(ns + ".action.stop.distance_threshold");
 
+  pp.ego_min_velocity = node.declare_parameter<double>(ns + ".ego.min_assumed_velocity");
   pp.extra_front_offset = node.declare_parameter<double>(ns + ".ego.extra_front_offset");
   pp.extra_rear_offset = node.declare_parameter<double>(ns + ".ego.extra_rear_offset");
   pp.extra_left_offset = node.declare_parameter<double>(ns + ".ego.extra_left_offset");

planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp

@@ -16,6 +16,7 @@
 
 #include "debug.hpp"
 #include "decisions.hpp"
+#include "filter_predicted_objects.hpp"
 #include "footprint.hpp"
 #include "lanelets_selection.hpp"
 #include "overlapping_range.hpp"
@@ -103,7 +104,7 @@ bool OutOfLaneModule::modifyPathVelocity(
   DecisionInputs inputs;
   inputs.ranges = ranges;
   inputs.ego_data = ego_data;
-  inputs.objects = *planner_data_->predicted_objects;
+  inputs.objects = filter_predicted_objects(*planner_data_->predicted_objects, ego_data, params_);
   inputs.route_handler = planner_data_->route_handler_;
   inputs.lanelets = other_lanelets;
   auto decisions = calculate_decisions(inputs, params_, logger_);

planning/behavior_velocity_out_of_lane_module/src/types.hpp

@@ -42,10 +42,11 @@ struct PlannerParam
   double intervals_ego_buffer;  // [s](mode="intervals") buffer to extend the ego time range
   double intervals_obj_buffer;  // [s](mode="intervals") buffer to extend the objects time range
   double ttc_threshold;  // [s](mode="ttc") threshold on time to collision between ego and an object
+  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_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
 
   double overlap_extra_length;  // [m] extra length to add around an overlap range
   double overlap_min_dist;      // [m] min distance inside another lane to consider an overlap