Ignore objects on trajectory

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

planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/footprints.hpp

@@ -17,13 +17,18 @@
 
 #include "types.hpp"
 
+#include <autoware/universe_utils/geometry/boost_polygon_utils.hpp>
 #include <autoware/universe_utils/geometry/geometry.hpp>
 #include <autoware/universe_utils/ros/uuid_helper.hpp>
 #include <autoware_vehicle_info_utils/vehicle_info.hpp>
 
 #include <autoware_perception_msgs/msg/predicted_object.hpp>
 
+#include <boost/geometry/algorithms/correct.hpp>
+#include <boost/geometry/algorithms/union.hpp>
+
 #include <Eigen/src/Geometry/Rotation2D.h>
+#include <Eigen/src/Geometry/RotationBase.h>
 
 #include <vector>
 
@@ -53,7 +58,7 @@ inline std::vector<run_out::ObjectFootprint> calculate_object_footprints(
   return footprints;
 }
 
-run_out::TrajectoryCornerFootprint calculate_trajectory_corner_footprint(
+inline run_out::TrajectoryCornerFootprint calculate_trajectory_corner_footprint(
   const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
   autoware::vehicle_info_utils::VehicleInfo vehicle_info, const run_out::Parameters & params)
 {
@@ -82,6 +87,28 @@ run_out::TrajectoryCornerFootprint calculate_trajectory_corner_footprint(
     footprint.rear_left_ls.emplace_back(
       base_link.x() + rotated_rear_left_offset.x(), base_link.y() + rotated_rear_left_offset.y());
   }
+
+  universe_utils::Polygon2d polygon_front;
+  for (const auto & p : footprint.front_left_ls) {
+    polygon_front.outer().push_back(p);
+  }
+  std::for_each(
+    footprint.front_right_ls.rbegin(), footprint.front_right_ls.rend(),
+    [&](const auto & p) { polygon_front.outer().push_back(p); });
+  boost::geometry::correct(polygon_front);
+  universe_utils::Polygon2d polygon_rear;
+  for (const auto & p : footprint.rear_left_ls) {
+    polygon_rear.outer().push_back(p);
+  }
+  std::for_each(
+    footprint.rear_right_ls.rbegin(), footprint.rear_right_ls.rend(),
+    [&](const auto & p) { polygon_rear.outer().push_back(p); });
+  boost::geometry::correct(polygon_rear);
+  universe_utils::MultiPolygon2d unions;
+  boost::geometry::union_(polygon_front, polygon_rear, unions);
+  if (!unions.empty()) {
+    footprint.polygon = unions.front();
+  }
   return footprint;
 }
 }  // namespace autoware::motion_velocity_planner::run_out

planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/objects_filtering.hpp

@@ -16,13 +16,18 @@
 #define OBJECTS_FILTERING_HPP_
 
 #include "parameters.hpp"
+#include "types.hpp"
 
+#include <autoware/universe_utils/geometry/boost_polygon_utils.hpp>
+#include <autoware/universe_utils/ros/uuid_helper.hpp>
 #include <magic_enum.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 <boost/geometry/algorithms/correct.hpp>
+
 #include <string>
 #include <vector>
 
@@ -45,13 +50,33 @@ inline bool has_classification_label(
   return false;
 }
 
+inline bool is_on_ego_trajectory(
+  const autoware_perception_msgs::msg::PredictedObject & object,
+  const TrajectoryCornerFootprint & ego_trajectory)
+{
+  const auto half_length = object.shape.dimensions.x;
+  const auto object_polygon = universe_utils::toFootprint(
+    object.kinematics.initial_pose_with_covariance.pose, half_length, half_length,
+    object.shape.dimensions.y);
+  return boost::geometry::overlaps(object_polygon, ego_trajectory.polygon);
+}
+
 inline bool skip_object_condition(
-  const autoware_perception_msgs::msg::PredictedObject & object, const Parameters & params)
+  const autoware_perception_msgs::msg::PredictedObject & object,
+  const std::optional<DecisionHistory> & prev_decisions,
+  const TrajectoryCornerFootprint & ego_trajectory, const Parameters & params)
 {
+  const auto & is_previous_target =
+    prev_decisions && (prev_decisions->decisions.back().type == stop ||
+                       (prev_decisions->decisions.back().collision.has_value() &&
+                        prev_decisions->decisions.back().collision->type == collision));
+  if (!is_previous_target && is_on_ego_trajectory(object, ego_trajectory)) {
+    return true;
+  }
   if (!has_classification_label(object, params.objects_target_labels)) {
-    return false;
+    return true;
   }
-  return true;
+  return false;
 }
 
 inline void filter_predicted_paths(
@@ -62,7 +87,9 @@ inline void filter_predicted_paths(
 }
 
 inline std::vector<autoware_perception_msgs::msg::PredictedObject> filter_dynamic_objects(
-  const autoware_perception_msgs::msg::PredictedObjects & objects, const Parameters & params)
+  const autoware_perception_msgs::msg::PredictedObjects & objects,
+  const ObjectDecisionsTracker & previous_decisions,
+  const TrajectoryCornerFootprint & ego_trajectory, const Parameters & params)
 {
   std::vector<autoware_perception_msgs::msg::PredictedObject> filtered_objects;
   std::vector<autoware_perception_msgs::msg::PredictedObject> parked_objects;
@@ -76,7 +103,9 @@ inline std::vector<autoware_perception_msgs::msg::PredictedObject> filter_dynami
       }
       continue;
     }
-    if (skip_object_condition(filtered_object, params)) {
+    const auto & previous_object_decisions =
+      previous_decisions.get(universe_utils::toHexString(object.object_id));
+    if (skip_object_condition(filtered_object, previous_object_decisions, ego_trajectory, params)) {
       continue;
     }
     filter_predicted_paths(filtered_object, params);

planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/parameters.hpp

@@ -54,10 +54,9 @@ struct Parameters
   void initialize(rclcpp::Node & node, const std::string & ns)
   {
     using universe_utils::getOrDeclareParameter;
-    enable_passing_collisions =
-      getOrDeclareParameter<bool>(node, ns + ".enable_passing_collisions");
+    enable_passing_collisions = getOrDeclareParameter<bool>(node, ns + ".passing.enable");
     passing_collisions_time_margin =
-      getOrDeclareParameter<double>(node, ns + ".passing_collisions_time_margin");
+      getOrDeclareParameter<double>(node, ns + ".passing.time_margin");
     stop_off_time_buffer = getOrDeclareParameter<double>(node, ns + ".stop_off_time_buffer");
     stop_on_time_buffer = getOrDeclareParameter<double>(node, ns + ".stop_on_time_buffer");
     stop_distance_buffer = getOrDeclareParameter<double>(node, ns + ".stop_distance_buffer");
@@ -78,8 +77,8 @@ struct Parameters
     using universe_utils::updateParam;
     updateParam(params, ns + ".stop_on_time_buffer", stop_on_time_buffer);
     updateParam(params, ns + ".stop_off_time_buffer", stop_off_time_buffer);
-    updateParam(params, ns + ".enable_passing_collisions", enable_passing_collisions);
-    updateParam(params, ns + ".passing_collisions_time_margin", passing_collisions_time_margin);
+    updateParam(params, ns + ".passing.enable", enable_passing_collisions);
+    updateParam(params, ns + ".passing.time_margin", passing_collisions_time_margin);
     updateParam(params, ns + ".stop_off_time_buffer", stop_off_time_buffer);
     updateParam(params, ns + ".stop_on_time_buffer", stop_on_time_buffer);
     updateParam(params, ns + ".stop_distance_buffer", stop_distance_buffer);

planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/run_out_module.cpp

@@ -144,38 +144,39 @@ VelocityPlanningResult RunOutModule::plan(
 {
   const auto now = clock_->now();
   time_keeper_->start_track("plan()");
-  time_keeper_->start_track("filter_objects()");
-  const auto filtered_objects =
-    run_out::filter_dynamic_objects(planner_data->predicted_objects, params_);
-  time_keeper_->end_track("filter_objects()");
   time_keeper_->start_track("calc_ego_footprint()");
   const auto ego_footprint = calculate_trajectory_corner_footprint(
     ego_trajectory_points, planner_data->vehicle_info_, params_);
   time_keeper_->end_track("calc_ego_footprint()");
+  time_keeper_->start_track("filter_objects()");
+  const auto filtered_objects = run_out::filter_dynamic_objects(
+    planner_data->predicted_objects, decisions_tracker_, ego_footprint, params_);
+  time_keeper_->end_track("filter_objects()");
   time_keeper_->start_track("calc_obj_footprint()");
-  const auto object_footprints = run_out::calculate_object_footprints(filtered_objects);
+  const auto object_footprints =
+    run_out::calculate_object_footprints(filtered_objects);  // TODO(Maxime): delete if not used
   time_keeper_->end_track("calc_obj_footprint()");
   time_keeper_->start_track("calc_rtree()");
   const auto footprint_rtree = run_out::prepare_trajectory_footprint_rtree(ego_footprint);
   time_keeper_->end_track("calc_rtree()");
   time_keeper_->start_track("calc_collisions()");
   auto collisions_per_object = run_out::calculate_collisions_per_object(
-    planner_data->predicted_objects.objects, footprint_rtree, ego_trajectory_points, params_);
+    filtered_objects, footprint_rtree, ego_trajectory_points, params_);
   time_keeper_->end_track("calc_collisions()");
   time_keeper_->start_track("calc_decisions()");
   run_out::calculate_decisions(decisions_tracker_, collisions_per_object, now, params_);
   time_keeper_->end_track("calc_decisions()");
   time_keeper_->start_track("calc_slowdowns()");
   const auto result = run_out::calculate_slowdowns(
     decisions_tracker_, ego_trajectory_points,
-    planner_data->vehicle_info_.front_overhang_m + params_.stop_distance_buffer);
+    params_.stop_distance_buffer + planner_data->vehicle_info_.max_longitudinal_offset_m);
   check_unavoidable_collision(calculate_comfortable_time_to_stop(
     ego_trajectory_points, planner_data->calculate_min_deceleration_distance(0.0)));
   time_keeper_->end_track("calc_slowdowns()");
 
   time_keeper_->start_track("publish_debug()");
   virtual_wall_marker_creator.add_virtual_walls(run_out::create_virtual_walls(
-    result, ego_trajectory_points, planner_data->vehicle_info_.front_overhang_m));
+    result, ego_trajectory_points, planner_data->vehicle_info_.max_longitudinal_offset_m));
   virtual_wall_publisher_->publish(virtual_wall_marker_creator.create_markers(now));
   debug_publisher_->publish(
     run_out::make_debug_footprint_markers(ego_footprint, object_footprints));

planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/slowdown.hpp

@@ -51,6 +51,7 @@ inline geometry_msgs::msg::Point interpolated_point_at_time(
   return universe_utils::calcInterpolatedPoint(
     prev_it->pose.position, std::next(prev_it)->pose.position, ratio);
 }
+
 inline VelocityPlanningResult calculate_slowdowns(
   ObjectDecisionsTracker & decision_tracker,
   const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,

planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/types.hpp

@@ -38,6 +38,7 @@ struct TrajectoryCornerFootprint
   universe_utils::LineString2d front_right_ls;
   universe_utils::LineString2d rear_left_ls;
   universe_utils::LineString2d rear_right_ls;
+  universe_utils::Polygon2d polygon;
 };
 
 struct ObjectFootprint
@@ -215,6 +216,14 @@ struct ObjectDecisionsTracker
       }
     }
   }
+
+  std::optional<DecisionHistory> get(const std::string & object) const
+  {
+    if (history_per_object.count(object) < 1) {
+      return std::nullopt;
+    }
+    return history_per_object.at(object);
+  }
 };
 
 struct UnavoidableCollision