feat(run_out): add uncertain mode for newly detected objects

planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/objects_filtering.cpp

@@ -17,18 +17,18 @@
 #include "parameters.hpp"
 #include "types.hpp"
 
+#include <autoware/interpolation/linear_interpolation.hpp>
+#include <autoware/motion_utils/trajectory/interpolation.hpp>
+#include <autoware/motion_utils/trajectory/trajectory.hpp>
 #include <autoware/motion_velocity_planner_common/planner_data.hpp>
 #include <autoware/universe_utils/geometry/geometry.hpp>
 #include <autoware/universe_utils/ros/uuid_helper.hpp>
 #include <autoware_utils_geometry/geometry.hpp>
 
-#include <autoware_perception_msgs/msg/detail/object_classification__struct.hpp>
-#include <autoware_perception_msgs/msg/detail/predicted_object__struct.hpp>
 #include <autoware_perception_msgs/msg/object_classification.hpp>
 #include <autoware_perception_msgs/msg/predicted_object.hpp>
 #include <autoware_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_perception_msgs/msg/predicted_path.hpp>
-#include <geometry_msgs/msg/detail/point__struct.hpp>
 
 #include <boost/geometry/algorithms/correct.hpp>
 #include <boost/geometry/algorithms/detail/overlaps/interface.hpp>
@@ -80,7 +80,7 @@
 }
 
 bool skip_object_condition(
-  Object & object, const std::optional<DecisionHistory> & prev_decisions,
+  Object & object, const std::optional<DecisionHistory> & prev_decisions, const bool is_uncertain,
   const universe_utils::Segment2d & ego_rear_segment, const FilteringData & filtering_data,
   const Parameters & parameters)
 {
@@ -104,7 +104,7 @@
   if (is_previous_target) {
     return !skip_object;
   }
-  if (params.ignore_if_stopped && object.is_stopped) {
+  if (params.ignore_if_stopped && object.is_stopped && !is_uncertain) {
     return skip_object;
   }
   if (!filtering_data.ignore_objects_rtree.is_geometry_disjoint_from_rtree_polygons(
@@ -140,8 +140,7 @@
 }
 
 void calculate_predicted_path_footprints(
-  Object & object, const autoware_perception_msgs::msg::PredictedObject & predicted_object,
-  const Parameters & params)
+  Object & object, const autoware_perception_msgs::msg::PredictedObject & predicted_object)
 {
   auto width = 0.0;
   auto half_length = 0.0;
@@ -158,8 +157,7 @@
     half_length = width / 2.0;
   }
   // calculate footprint
-  for (const auto & path :
-       filter_by_confidence(predicted_object.kinematics.predicted_paths, object.label, params)) {
+  for (const auto & path : predicted_object.kinematics.predicted_paths) {
     ObjectPredictedPathFootprint footprint;
     footprint.time_step = rclcpp::Duration(path.time_step).seconds();
     for (const auto & p : path.path) {
@@ -177,6 +175,42 @@
   }
 }
 
+void recalculate_predicted_paths(
+  autoware_perception_msgs::msg::PredictedObject & object, const double min_vel)
+{
+  for (auto & path : object.kinematics.predicted_paths) {
+    auto base_path = path.path;
+    path.path.clear();
+    std::vector<double> arc_lengths = {0.0};
+    std::vector<double> recalculated_arc_lengths = {0.0};
+    arc_lengths.reserve(base_path.size());
+    const auto min_ds = min_vel * rclcpp::Duration(path.time_step).seconds();
+    for (size_t i = 0; i + 1 < base_path.size(); ++i) {
+      const auto ds = autoware_utils_geometry::calc_distance2d(base_path[i], base_path[i + 1]);
+      arc_lengths.push_back(arc_lengths.back() + ds);
+      recalculated_arc_lengths.push_back(recalculated_arc_lengths.back() + std::max(min_ds, ds));
+    }
+    if (recalculated_arc_lengths.back() > arc_lengths.back()) {
+      const auto last_ds = arc_lengths.back() - arc_lengths[arc_lengths.size() - 2];
+      const auto ratio = (last_ds + recalculated_arc_lengths.back() - arc_lengths.back()) / last_ds;
+      const auto extended_x = autoware::interpolation::lerp(
+        base_path[base_path.size() - 2].position.x, base_path[base_path.size() - 1].position.y,
+        ratio);
+      const auto extended_y = autoware::interpolation::lerp(
+        base_path[base_path.size() - 2].position.y, base_path[base_path.size() - 1].position.y,
+        ratio);
+      auto extended_pose = base_path.back();
+      extended_pose.position.x = extended_x;
+      extended_pose.position.y = extended_y;
+      base_path.push_back(extended_pose);
+      arc_lengths.push_back(recalculated_arc_lengths.back());
+    }
+    for (const auto s : recalculated_arc_lengths) {
+      path.path.push_back(autoware::motion_utils::calcInterpolatedPose(base_path, s));
+    }
+  }
+}
+
 void cut_predicted_path_footprint(
   ObjectPredictedPathFootprint & footprint, const size_t cut_index,
   const double standstill_duration_after_cut)
@@ -276,7 +310,8 @@
 std::vector<Object> prepare_dynamic_objects(
   const std::vector<std::shared_ptr<motion_velocity_planner::PlannerData::Object>> & objects,
   const TrajectoryCornerFootprint & ego_trajectory,
-  const ObjectDecisionsTracker & previous_decisions, const FilteringDataPerLabel & filtering_data,
+  const ObjectDecisionsTracker & previous_decisions,
+  const ObjectDetectionTracker & detection_tracker, const FilteringDataPerLabel & filtering_data,
   const Parameters & params)
 {
   std::vector<Object> filtered_objects;
@@ -292,13 +327,24 @@
         .to_2d();
     classify(filtered_object, object->predicted_object, target_labels, params);
     calculate_current_footprint(filtered_object, object->predicted_object);
+    const auto & detection_duration =
+      detection_tracker.get_detection_duration(filtered_object.uuid);
+    const auto is_uncertain =
+      detection_duration &&
+      *detection_duration < params.objects_uncertain_mode_detection_duration_threshold;
     const auto & previous_object_decisions = previous_decisions.get(filtered_object.uuid);
     if (skip_object_condition(
-          filtered_object, previous_object_decisions, ego_rear_segment,
+          filtered_object, previous_object_decisions, is_uncertain, ego_rear_segment,
           filtering_data[filtered_object.label], params)) {
       continue;
     }
-    calculate_predicted_path_footprints(filtered_object, object->predicted_object, params);
+    auto predicted_object = object->predicted_object;
+    predicted_object.kinematics.predicted_paths = filter_by_confidence(
+      predicted_object.kinematics.predicted_paths, filtered_object.label, params);
+    if (is_uncertain) {
+      recalculate_predicted_paths(predicted_object, params.objects_uncertain_mode_min_velocity);
+    }
+    calculate_predicted_path_footprints(filtered_object, predicted_object);
     filter_predicted_paths(
       filtered_object, filtering_data[filtered_object.label],
       params.object_parameters_per_label[filtered_object.label]);

planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/objects_filtering.hpp

@@ -64,8 +64,7 @@ void cut_predicted_path_footprint(
 
 /// @brief calculate the predicted path footprints of an object
 void calculate_predicted_path_footprints(
-  Object & object, const autoware_perception_msgs::msg::PredictedObject & predicted_object,
-  const Parameters & params);
+  Object & object, const autoware_perception_msgs::msg::PredictedObject & predicted_object);
 
 /// @brief calculate the first index of a predicted path that crosses a cut line in the map data
 std::optional<size_t> get_cut_predicted_path_index(
@@ -81,7 +80,8 @@ void filter_predicted_paths(
 std::vector<Object> prepare_dynamic_objects(
   const std::vector<std::shared_ptr<motion_velocity_planner::PlannerData::Object>> & objects,
   const TrajectoryCornerFootprint & ego_trajectory,
-  const ObjectDecisionsTracker & previous_decisions, const FilteringDataPerLabel & filtering_data,
+  const ObjectDecisionsTracker & previous_decisions,
+  const ObjectDetectionTracker & detection_tracker, const FilteringDataPerLabel & filtering_data,
   const Parameters & params);
 }  // namespace autoware::motion_velocity_planner::run_out
 

planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/parameters.hpp

@@ -132,6 +132,12 @@ struct Parameters
   // object parameters
   std::vector<std::string> objects_target_labels;
   std::vector<ObjectParameters> object_parameters_per_label;
+  double
+    objects_uncertain_mode_detection_duration_threshold;  // [s] conservative mode is used towards
+                                                          // objects that are detected for a
+                                                          // duration shorter than this threshold
+  double objects_uncertain_mode_min_velocity;  // [m/s] minimum velocity used along the predicted
+                                               // path of an object when using the uncertain mode
 
   struct
   {
@@ -214,6 +220,10 @@ struct Parameters
       *std::max_element(all_object_labels.begin(), all_object_labels.end()) + 1);
     objects_target_labels =
       getOrDeclareParameter<std::vector<std::string>>(node, ns + ".objects.target_labels");
+    objects_uncertain_mode_detection_duration_threshold = getOrDeclareParameter<double>(
+      node, ns + ".objects.uncertain_mode.detection_duration_threshold");
+    objects_uncertain_mode_min_velocity =
+      getOrDeclareParameter<double>(node, ns + ".objects.uncertain_mode.minimum_velocity");
     for (const auto label : all_object_labels) {
       object_parameters_per_label[label].ignore_if_on_ego_trajectory =
         get_object_parameter<bool>(node, ns, label, ".ignore.if_on_ego_trajectory");

planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/run_out_module.cpp

@@ -176,8 +176,10 @@
   time_keeper_->start_track("filter_objects()");
   const auto filtering_data = run_out::calculate_filtering_data(
     planner_data->route_handler->getLaneletMapPtr(), ego_footprint, planner_data->objects, params_);
+  detection_tracker_.update(planner_data->objects, now);
   auto filtered_objects = run_out::prepare_dynamic_objects(
-    planner_data->objects, ego_footprint, decisions_tracker_, filtering_data, params_);
+    planner_data->objects, ego_footprint, decisions_tracker_, detection_tracker_, filtering_data,
+    params_);
   time_keeper_->end_track("filter_objects()");
   time_keeper_->start_track("calc_collisions()");
   params_.ignore_collision_conditions.if_ego_arrives_first_and_cannot_stop

planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/run_out_module.hpp

@@ -81,6 +81,7 @@ class RunOutModule : public PluginModuleInterface
   std::optional<double> unfeasible_stop_deceleration_;
 
   run_out::ObjectDecisionsTracker decisions_tracker_;
+  run_out::ObjectDetectionTracker detection_tracker_;
   run_out::Parameters params_{};
 
   /// @brief update whether we are currently inserting a stop that breaks the deceleration limit

planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/types.hpp

@@ -18,6 +18,7 @@
 #include <autoware/motion_velocity_planner_common/planner_data.hpp>
 #include <autoware/motion_velocity_planner_common/velocity_planning_result.hpp>
 #include <autoware/universe_utils/geometry/boost_geometry.hpp>
+#include <autoware/universe_utils/ros/uuid_helper.hpp>
 #include <rclcpp/time.hpp>
 
 #include <autoware_internal_planning_msgs/msg/safety_factor_array.hpp>
@@ -36,6 +37,7 @@
 #include <optional>
 #include <string>
 #include <unordered_map>
+#include <unordered_set>
 #include <utility>
 #include <vector>
 
@@ -288,6 +290,47 @@ struct ObjectDecisionsTracker
     return history_per_object.at(object);
   }
 };
+/// @brief Track the duration since an object is being detected
+class ObjectDetectionTracker
+{
+  rclcpp::Time current_time_;
+  std::unordered_map<std::string, rclcpp::Time> first_detection_times_;
+
+  /// @brief update the tracker, adding new objects and removing no longer detected ones
+public:
+  void update(
+    const std::vector<std::shared_ptr<motion_velocity_planner::PlannerData::Object>> & objects,
+    const rclcpp::Time & current_time)
+  {
+    current_time_ = current_time;
+    std::unordered_set<std::string> current_uuids;
+    current_uuids.reserve(objects.size());
+    for (const auto & object : objects) {
+      const auto uuid = universe_utils::toHexString(object->predicted_object.object_id);
+      current_uuids.insert(uuid);
+      first_detection_times_.try_emplace(uuid, current_time);
+    }
+
+    for (auto it = first_detection_times_.begin(); it != first_detection_times_.end();) {
+      if (current_uuids.find(it->first) == current_uuids.end()) {
+        it = first_detection_times_.erase(it);
+      } else {
+        ++it;
+      }
+    }
+  }
+
+  /// @brief return the duration since first detecting the given object
+  std::optional<double> get_detection_duration(const std::string & uuid) const
+  {
+    const auto it = first_detection_times_.find(uuid);
+    if (it == first_detection_times_.end()) {
+      return std::nullopt;
+    }
+    return (current_time_ - it->second).seconds();
+  }
+};
+
 /// @brief Object represented by its uuid, corner footprints, current footprint, position,
 /// collisions with ego, classification label
 struct Object