feat(out_of_lane): ignore dynamic objects located in irrelevent lanel…

…ets (#4473)

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

planning/behavior_velocity_out_of_lane_module/src/decisions.cpp

@@ -40,10 +40,31 @@ double time_along_path(const EgoData & ego_data, const size_t target_idx)
   return dist / v;
 }
 
+bool object_is_incoming(
+  const lanelet::BasicPoint2d & object_position,
+  const std::shared_ptr<route_handler::RouteHandler> route_handler,
+  const lanelet::ConstLanelet & lane)
+{
+  const auto lanelets = route_handler->getPrecedingLaneletSequence(lane, 50.0);
+  if (boost::geometry::within(object_position, lane.polygon2d().basicPolygon())) return true;
+  for (const auto & lls : lanelets)
+    for (const auto & ll : lls)
+      if (boost::geometry::within(object_position, ll.polygon2d().basicPolygon())) return true;
+  return false;
+}
+
 std::optional<std::pair<double, double>> object_time_to_range(
   const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range,
-  const double min_confidence, const rclcpp::Logger & logger)
+  const std::shared_ptr<route_handler::RouteHandler> route_handler, const double min_confidence,
+  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
+  const auto object_point = lanelet::BasicPoint2d(
+    object.kinematics.initial_pose_with_covariance.pose.position.x,
+    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;
 
   auto worst_enter_time = std::optional<double>();
@@ -244,7 +265,7 @@ bool ttc_condition(
   return collision_during_overlap || ttc_is_bellow_threshold;
 }
 
-bool object_is_incoming(
+bool will_collide_on_range(
   const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger)
 {
   RCLCPP_DEBUG(
@@ -277,7 +298,8 @@ 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, params.objects_min_confidence, logger)
+        ? object_time_to_range(
+            object, range, inputs.route_handler, 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");
@@ -286,7 +308,7 @@ bool should_not_enter(
 
     range_times.object.enter_time = enter_exit_time->first;
     range_times.object.exit_time = enter_exit_time->second;
-    if (object_is_incoming(range_times, params, logger)) return true;
+    if (will_collide_on_range(range_times, params, logger)) return true;
   }
   return false;
 }

planning/behavior_velocity_out_of_lane_module/src/decisions.hpp

@@ -56,18 +56,21 @@ 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] 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 double min_confidence, const rclcpp::Logger & logger);
+  const std::shared_ptr<route_handler::RouteHandler> route_handler, 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] inputs information used to take decisions (ranges, ego and objects data, route
+/// handler, lanelets)
 /// @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.
@@ -79,7 +82,7 @@ std::optional<std::pair<double, double>> object_time_to_range(
 /// @param [in] range_times times when ego and the object enter/exit the range
 /// @param [in] params parameters
 /// @param [in] logger ros logger
-bool object_is_incoming(
+bool will_collide_on_range(
   const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger);
 /// @brief check whether we should avoid entering the given range
 /// @param [in] range the range to check