From 19553ac8143c7a6aed35ae891c6220d522ad59ef Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Thu, 3 Aug 2023 16:36:32 +0900 Subject: [PATCH] feat(out_of_lane): ignore dynamic objects located in irrelevent lanelets (#4473) Signed-off-by: Maxime CLEMENT --- .../src/decisions.cpp | 30 ++++++++++++++++--- .../src/decisions.hpp | 9 ++++-- 2 files changed, 32 insertions(+), 7 deletions(-) diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp index 0fdd153b17530..163c8632e40c7 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp +++ b/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, + 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> 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, 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(); @@ -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; } diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp index da183133e7728..bf6bf81e506cf 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp +++ b/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> 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, 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> 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