feat(out_of_lane): add param for the min confidence of a predicted pa…

…th (autowarefoundation#4217)

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

planning/behavior_velocity_out_of_lane_module/README.md

@@ -101,7 +101,7 @@ at its current velocity or at half the velocity of the path points, whichever is
 ###### Dynamic objects
 
 Two methods are used to estimate the time when a dynamic objects with reach some point.
-If `objects.use_predicted_paths` is set to `true`, the predicted path of the dynamic object is used.
+If `objects.use_predicted_paths` is set to `true`, the predicted paths of the dynamic object are used if their confidence value is higher than the value set by the `objects.predicted_path_min_confidence` parameter.
 Otherwise, the lanelet map is used to estimate the distance between the object and the point and the time is calculated assuming the object keeps its current velocity.
 
 #### 5. Path update
@@ -138,10 +138,11 @@ Moreover, parameter `action.distance_buffer` adds an extra distance between the
 | -------------- | ------ | ------------------------------------------------------------------------------------------------------ |
 | `threshold`    | double | [s] consider objects with an estimated time to collision bellow this value while ego is on the overlap |
 
-| Parameter /objects    | Type   | Description                                                                                                                                                               |
-| --------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
-| `minimum_velocity`    | double | [m/s] consider objects with an estimated time to collision bellow this value while on the overlap                                                                         |
-| `use_predicted_paths` | bool   | [-] 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 |
+| Parameter /objects              | Type   | Description                                                                                                                                                               |
+| ------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
+| `minimum_velocity`              | double | [m/s] consider objects with an estimated time to collision bellow this value while on the overlap                                                                         |
+| `use_predicted_paths`           | bool   | [-] 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` | double | [-] minimum confidence required for a predicted path to be considered                                                                                                     |
 
 | Parameter /overlap | Type   | Description                                                                                          |
 | ------------------ | ------ | ---------------------------------------------------------------------------------------------------- |

planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml

@@ -16,6 +16,7 @@
         minimum_velocity: 0.5  # [m/s] objects lower than this velocity will be ignored
         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.
 
       overlap:
         minimum_distance: 0.0  # [m] minimum distance inside a lanelet for an overlap to be considered

planning/behavior_velocity_out_of_lane_module/src/decisions.cpp

@@ -42,14 +42,15 @@ double time_along_path(const EgoData & ego_data, const size_t target_idx)
 
 std::optional<std::pair<double, double>> object_time_to_range(
   const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range,
-  const rclcpp::Logger & logger)
+  const double min_confidence, const rclcpp::Logger & logger)
 {
   const auto max_deviation = object.shape.dimensions.y * 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) {
+    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());
@@ -274,9 +275,10 @@ bool should_not_enter(
       continue;  // skip objects with velocity bellow a threshold
     }
     // skip objects that are already on the interval
-    const auto enter_exit_time = params.objects_use_predicted_paths
-                                   ? object_time_to_range(object, range, logger)
-                                   : object_time_to_range(object, range, inputs, logger);
+    const auto enter_exit_time =
+      params.objects_use_predicted_paths
+        ? object_time_to_range(object, range, params.objects_min_confidence, logger)
+        : object_time_to_range(object, range, inputs, logger);
     if (!enter_exit_time) {
       RCLCPP_DEBUG(logger, " SKIP (no enter/exit times found)\n");
       continue;  // skip objects that are not driving towards the overlapping range

planning/behavior_velocity_out_of_lane_module/src/decisions.hpp

@@ -56,22 +56,24 @@ double time_along_path(const EgoData & ego_data, const size_t target_idx);
 /// but may not exist (e.g,, predicted path ends before reaching the end of the range)
 /// @param [in] object dynamic object
 /// @param [in] range overlapping range
+/// @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 autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range,
+  const double min_confidence, 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] lanelets objects to consider
-/// @param [in] route_handler route handler used to estimate the path of the dynamic object
+/// @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 lanelet::ConstLanelets & lanelets,
-  const std::shared_ptr<route_handler::RouteHandler> & route_handler);
+  const DecisionInputs & inputs, const rclcpp::Logger & logger);
 /// @brief decide whether an object is coming in the range at the same time as ego
 /// @details the condition depends on the mode (threshold, intervals, ttc)
 /// @param [in] range_times times when ego and the object enter/exit the range

planning/behavior_velocity_out_of_lane_module/src/manager.cpp

@@ -44,6 +44,8 @@ OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node)
   pp.objects_min_vel = node.declare_parameter<double>(ns + ".objects.minimum_velocity");
   pp.objects_use_predicted_paths =
     node.declare_parameter<bool>(ns + ".objects.use_predicted_paths");
+  pp.objects_min_confidence =
+    node.declare_parameter<double>(ns + ".objects.predicted_path_min_confidence");
 
   pp.overlap_min_dist = node.declare_parameter<double>(ns + ".overlap.minimum_distance");
   pp.overlap_extra_length = node.declare_parameter<double>(ns + ".overlap.extra_length");

planning/behavior_velocity_out_of_lane_module/src/types.hpp

@@ -45,6 +45,7 @@ struct PlannerParam
 
   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