From ff0ca7a202828ef1b195ae32bb11cb17e9725c18 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Tue, 21 Nov 2023 22:41:26 +0900 Subject: [PATCH] additional filters and heavy revamp Signed-off-by: Muhammad Zulfaqar Azmi --- .../scene_module/lane_change/normal.hpp | 2 +- .../lane_change/lane_change_module_data.hpp | 14 +- .../utils/lane_change/utils.hpp | 4 + .../path_safety_checker/objects_filtering.hpp | 22 ++- .../path_safety_checker_parameters.hpp | 10 +- .../goal_planner/goal_planner_module.cpp | 2 +- .../src/scene_module/lane_change/manager.cpp | 5 + .../src/scene_module/lane_change/normal.cpp | 158 +++++------------- .../start_planner/start_planner_module.cpp | 3 +- .../src/utils/lane_change/utils.cpp | 45 +++++ .../path_safety_checker/objects_filtering.cpp | 73 +++++--- 11 files changed, 182 insertions(+), 156 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index 4a117e5350baf..3bb7ca84e1f1b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -153,7 +153,7 @@ class NormalLaneChange : public LaneChangeBase const utils::path_safety_checker::RSSparams & rss_params, const bool is_stuck, CollisionCheckDebugMap & debug_data) const; - LaneChangeTargetObjectIndices filterObject( + LaneChangeTargetObjects filterObject( const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelets & target_backward_lanes) const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index 5a042b82919d6..4803248adc34a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -44,6 +44,13 @@ struct SafetyCheck double lane_expansion_left_offset{0.0}; double lane_expansion_right_offset{0.0}; }; + +struct LanesPolygon +{ + lanelet::BasicPolygon2d current_polygon; + lanelet::BasicPolygon2d target_polygon; + lanelet::BasicPolygons2d target_backward_polygons; +}; } // namespace behavior_path_planner::lane_change namespace behavior_path_planner @@ -136,13 +143,6 @@ struct LaneChangeInfo double terminal_lane_changing_velocity{0.0}; }; -struct LaneChangeTargetObjectIndices -{ - std::vector current_lane{}; - std::vector target_lane{}; - std::vector other_lane{}; -}; - struct LaneChangeTargetObjects { std::vector current_lane{}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index 5980a6a79fb79..f0cf3097cc040 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -39,6 +39,7 @@ using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_perception_msgs::msg::PredictedPath; using autoware_auto_planning_msgs::msg::PathWithLaneId; +using behavior_path_planner::lane_change::LanesPolygon; using behavior_path_planner::lane_change::PathSafetyStatus; using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped; @@ -202,5 +203,8 @@ lanelet::ConstLanelets generateExpandedLanelets( const lanelet::ConstLanelets & lanes, const Direction direction, const double left_offset, const double right_offset); +LanesPolygon createPolygon( + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, + const lanelet::ConstLanelets & target_backward_lanes); } // namespace behavior_path_planner::utils::lane_change #endif // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp index a8b09e6c15acc..566c975d021c9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp @@ -33,6 +33,21 @@ #include #include +namespace behavior_path_planner::utils::path_safety_checker::filters +{ + +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; + +bool position_filter( + const PredictedObject & object, const std::vector & path_points, + const geometry_msgs::msg::Point & current_pose, const double backward_distance, + const double forward_distance); + +bool velocity_filter( + const PredictedObject & object, double velocity_threshold, double max_velocity); +} // namespace behavior_path_planner::utils::path_safety_checker::filters + namespace behavior_path_planner::utils::path_safety_checker { @@ -71,8 +86,7 @@ bool isPolygonOverlapLanelet(const PredictedObject & object, const lanelet::Cons PredictedObjects filterObjects( const std::shared_ptr & objects, const std::shared_ptr & route_handler, const lanelet::ConstLanelets & current_lanes, - const geometry_msgs::msg::Point & current_pose, - const std::shared_ptr & params); + const geometry_msgs::msg::Point & current_pose, const ObjectsFilteringParams & params); /** * @brief Filters objects based on their velocity. @@ -118,8 +132,8 @@ PredictedObjects filterObjectsByVelocity( */ void filterObjectsByPosition( PredictedObjects & objects, const std::vector & path_points, - const geometry_msgs::msg::Point & current_pose, const double forward_distance, - const double backward_distance); + const geometry_msgs::msg::Point & current_pose, const double backward_distance, + const double forward_distance); /** * @brief Filters the provided objects based on their classification. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp index 3634586c40540..5d13a9da25a3d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -160,10 +160,12 @@ struct ObjectsFilteringParams double ignore_object_velocity_threshold{0.0}; ///< Velocity threshold for ignoring objects. ObjectTypesToCheck object_types_to_check{}; ///< Specifies which object types to check. ObjectLaneConfiguration object_lane_configuration{}; ///< Configuration for which lanes to check. - bool include_opposite_lane{false}; ///< Include the opposite lane in checks. - bool invert_opposite_lane{false}; ///< Invert the opposite lane in checks. - bool check_all_predicted_path{false}; ///< Check all predicted paths. - bool use_all_predicted_path{false}; ///< Use all predicted paths. + bool remove_velocity_above_threshold{ + false}; ///< Remove object with velocity above ignore_object_velocity_threshold + bool include_opposite_lane{false}; ///< Include the opposite lane in checks. + bool invert_opposite_lane{false}; ///< Invert the opposite lane in checks. + bool check_all_predicted_path{false}; ///< Check all predicted paths. + bool use_all_predicted_path{false}; ///< Use all predicted paths. bool use_predicted_path_outside_lanelet{false}; ///< Use predicted paths outside of lanelets. }; diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index ea8b5fd79c5d0..a471968938cba 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -1787,7 +1787,7 @@ bool GoalPlannerModule::isSafePath() const // filtering objects with velocity, position and class const auto filtered_objects = utils::path_safety_checker::filterObjects( dynamic_object, route_handler, pull_over_lanes, current_pose.position, - objects_filtering_params_); + *objects_filtering_params_); // filtering objects based on the current position's lane const auto target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane( diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index eb2c7941f4a46..8c6f201507e21 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -119,6 +119,11 @@ LaneChangeModuleManager::LaneChangeModuleManager( getOrDeclareParameter(*node, parameter("safety_check.allow_loose_check_for_cancel")); } + { + p.safety_check.object_filtering.object_check_forward_distance = + getOrDeclareParameter(*node, "forward_path_length"); + } + { const auto rss_params = [&node](const std::string & phase) { utils::path_safety_checker::RSSparams params; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 4290b51e7aeb2..9fa0073a68a7e 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -778,173 +778,101 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const { const auto current_pose = getEgoPose(); - const auto & route_handler = *getRouteHandler(); - const auto & common_parameters = planner_data_->parameters; auto objects = *planner_data_->dynamic_object; utils::path_safety_checker::filterObjectsByClass( objects, lane_change_parameters_->safety_check.object_filtering.object_types_to_check); + const auto & route_handler = getRouteHandler(); + { + using utils::path_safety_checker::filters::position_filter; + using + const auto path = + route_handler->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + utils::path_safety_checker::filterObjects(objects, [&](const auto & object) { + return !utils::path_safety_checker::filters::position_filter( + object, path.points, getEgoPosition(), std::numeric_limits::lowest(), 0.0); + }); + } // get backward lanes const auto backward_length = lane_change_parameters_->backward_lane_length; const auto target_backward_lanes = utils::lane_change::getBackwardLanelets( - route_handler, target_lanes, current_pose, backward_length); + *route_handler, target_lanes, current_pose, backward_length); // filter objects to get target object index - const auto target_obj_index = - filterObject(objects, current_lanes, target_lanes, target_backward_lanes); - - LaneChangeTargetObjects target_objects; - target_objects.current_lane.reserve(target_obj_index.current_lane.size()); - target_objects.target_lane.reserve(target_obj_index.target_lane.size()); - target_objects.other_lane.reserve(target_obj_index.other_lane.size()); - - // objects in current lane - for (const auto & obj_idx : target_obj_index.current_lane) { - const auto extended_object = utils::lane_change::transform( - objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_); - target_objects.current_lane.push_back(extended_object); - } - - // objects in target lane - for (const auto & obj_idx : target_obj_index.target_lane) { - const auto extended_object = utils::lane_change::transform( - objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_); - target_objects.target_lane.push_back(extended_object); - } - - // objects in other lane - for (const auto & obj_idx : target_obj_index.other_lane) { - const auto extended_object = utils::lane_change::transform( - objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_); - target_objects.other_lane.push_back(extended_object); - } - - return target_objects; + return filterObject(objects, current_lanes, target_lanes, target_backward_lanes); } -LaneChangeTargetObjectIndices NormalLaneChange::filterObject( +LaneChangeTargetObjects NormalLaneChange::filterObject( const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelets & target_backward_lanes) const { - const auto current_pose = getEgoPose(); - const auto & route_handler = *getRouteHandler(); - const auto & common_parameters = planner_data_->parameters; - // Guard if (objects.objects.empty()) { return {}; } - // Get path - const auto path = - route_handler.getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); - const auto target_path = - route_handler.getCenterLinePath(target_lanes, 0.0, std::numeric_limits::max()); + const auto current_pose = getEgoPose(); + const auto & common_parameters = planner_data_->parameters; - const auto current_polygon = - utils::lane_change::createPolygon(current_lanes, 0.0, std::numeric_limits::max()); const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets( target_lanes, direction_, lane_change_parameters_->safety_check.lane_expansion_left_offset, lane_change_parameters_->safety_check.lane_expansion_right_offset); - const auto target_polygon = utils::lane_change::createPolygon( - expanded_target_lanes, 0.0, std::numeric_limits::max()); + const auto lanes_polygon = + utils::lane_change::createPolygon(current_lanes, expanded_target_lanes, target_backward_lanes); + const auto dist_ego_to_current_lanes_center = lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); - std::vector> target_backward_polygons; - for (const auto & target_backward_lane : target_backward_lanes) { - // Check to see is target_backward_lane is in current_lanes - // Without this check, current lane object might be treated as target lane object - const auto is_current_lane = [&](const lanelet::ConstLanelet & current_lane) { - return current_lane.id() == target_backward_lane.id(); - }; - if (std::any_of(current_lanes.begin(), current_lanes.end(), is_current_lane)) { - continue; - } + LaneChangeTargetObjects filtered_objects; + filtered_objects.current_lane.reserve(objects.objects.size()); + filtered_objects.target_lane.reserve(objects.objects.size()); + filtered_objects.other_lane.reserve(objects.objects.size()); - lanelet::ConstLanelets lanelet{target_backward_lane}; - auto lane_polygon = - utils::lane_change::createPolygon(lanelet, 0.0, std::numeric_limits::max()); - target_backward_polygons.push_back(lane_polygon); - } + const auto create_extended_object = [&](const auto & object) { + return utils::lane_change::transform(object, common_parameters, *lane_change_parameters_); + }; - LaneChangeTargetObjectIndices filtered_obj_indices; - for (size_t i = 0; i < objects.objects.size(); ++i) { - const auto & object = objects.objects.at(i); - const auto obj_velocity_norm = std::hypot( - object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.kinematics.initial_twist_with_covariance.twist.linear.y); + for (const auto & object : objects.objects) { const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); - // calc distance from the current ego position - double max_dist_ego_to_obj = std::numeric_limits::lowest(); - double min_dist_ego_to_obj = std::numeric_limits::max(); - const auto obj_polygon_outer = obj_polygon.outer(); - for (const auto & polygon_p : obj_polygon_outer) { - const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); - const double dist_ego_to_obj = calcSignedArcLength(path.points, current_pose.position, obj_p); - max_dist_ego_to_obj = std::max(dist_ego_to_obj, max_dist_ego_to_obj); - min_dist_ego_to_obj = std::min(dist_ego_to_obj, min_dist_ego_to_obj); - } + const auto check_intersection = [&obj_polygon](const auto & polygon) { + return !polygon.empty() && boost::geometry::intersects(polygon, obj_polygon); + }; - const auto is_lateral_far = [&]() { + const auto is_lateral_far = std::invoke([&]() { const auto dist_object_to_current_lanes_center = lanelet::utils::getLateralDistanceToClosestLanelet( current_lanes, object.kinematics.initial_pose_with_covariance.pose); const auto lateral = dist_object_to_current_lanes_center - dist_ego_to_current_lanes_center; return std::abs(lateral) > (common_parameters.vehicle_width / 2); - }; - - // ignore static object that are behind the ego vehicle - if (obj_velocity_norm < 1.0 && max_dist_ego_to_obj < 0.0) { - continue; - } + }); // check if the object intersects with target lanes - if (target_polygon && boost::geometry::intersects(target_polygon.value(), obj_polygon)) { - // TODO(watanabe): ignore static parked object that are in front of the ego vehicle in target - // lanes - - if (max_dist_ego_to_obj >= 0 || is_lateral_far()) { - filtered_obj_indices.target_lane.push_back(i); - continue; - } + if (check_intersection(lanes_polygon.target_polygon) && is_lateral_far) { + filtered_objects.target_lane.push_back(create_extended_object(object)); + continue; } - const auto check_backward_polygon = [&obj_polygon](const auto & target_backward_polygon) { - return target_backward_polygon && - boost::geometry::intersects(target_backward_polygon.value(), obj_polygon); - }; - // check if the object intersects with target backward lanes - if ( - !target_backward_polygons.empty() && - std::any_of( - target_backward_polygons.begin(), target_backward_polygons.end(), check_backward_polygon)) { - filtered_obj_indices.target_lane.push_back(i); + if (std::any_of( + lanes_polygon.target_backward_polygons.begin(), + lanes_polygon.target_backward_polygons.end(), check_intersection)) { + filtered_objects.target_lane.push_back(create_extended_object(object)); continue; } // check if the object intersects with current lanes - if ( - current_polygon && boost::geometry::intersects(current_polygon.value(), obj_polygon) && - max_dist_ego_to_obj >= 0.0) { + if (check_intersection(lanes_polygon.current_polygon)) { // check only the objects that are in front of the ego vehicle - filtered_obj_indices.current_lane.push_back(i); + filtered_objects.current_lane.push_back(create_extended_object(object)); continue; } - // ignore all objects that are behind the ego vehicle and not on the current and target - // lanes - if (max_dist_ego_to_obj < 0.0) { - continue; - } - - filtered_obj_indices.other_lane.push_back(i); + filtered_objects.other_lane.push_back(create_extended_object(object)); } - return filtered_obj_indices; + return filtered_objects; } std::vector NormalLaneChange::filterObjectsInTargetLane( diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 396e39eebba5f..fa19caedcbb7a 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -1077,7 +1077,8 @@ bool StartPlannerModule::isSafePath() const // filtering objects with velocity, position and class const auto filtered_objects = utils::path_safety_checker::filterObjects( - dynamic_object, route_handler, current_lanes, current_pose.position, objects_filtering_params_); + dynamic_object, route_handler, current_lanes, current_pose.position, + *objects_filtering_params_); // filtering objects based on the current position's lane const auto target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane( diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index 5694fc5bc48c4..03cac34edfb4c 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -1129,4 +1129,49 @@ lanelet::ConstLanelets generateExpandedLanelets( const auto right_extend_offset = (direction == Direction::RIGHT) ? -right_offset : 0.0; return lanelet::utils::getExpandedLanelets(lanes, left_extend_offset, right_extend_offset); } + +LanesPolygon createPolygon( + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, + const lanelet::ConstLanelets & target_backward_lanes) +{ + const auto current_polygon = + utils::lane_change::createPolygon(current_lanes, 0.0, std::numeric_limits::max()); + + const auto target_polygon = + utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits::max()); + + lanelet::BasicPolygons2d target_backward_polygons; + + for (const auto & target_backward_lane : target_backward_lanes) { + // Check to see is target_backward_lane is in current_lanes + // Without this check, current lane object might be treated as target lane object + const auto is_current_lane = [&](const lanelet::ConstLanelet & current_lane) { + return current_lane.id() == target_backward_lane.id(); + }; + + if (std::any_of(current_lanes.begin(), current_lanes.end(), is_current_lane)) { + continue; + } + + lanelet::ConstLanelets lanelet{target_backward_lane}; + auto lane_polygon = + utils::lane_change::createPolygon(lanelet, 0.0, std::numeric_limits::max()); + if (lane_polygon.has_value()) { + target_backward_polygons.push_back(lane_polygon.value()); + } + } + + LanesPolygon lanes_polygon; + if (current_polygon.has_value()) { + lanes_polygon.current_polygon = current_polygon.value(); + } + + if (target_polygon.has_value()) { + lanes_polygon.target_polygon = target_polygon.value(); + } + + lanes_polygon.target_backward_polygons = target_backward_polygons; + + return lanes_polygon; +} } // namespace behavior_path_planner::utils::lane_change diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp index 5a6d001c0af9a..0afee8b424386 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp @@ -19,9 +19,41 @@ #include #include +#include #include +namespace behavior_path_planner::utils::path_safety_checker::filters +{ + +bool position_filter( + const PredictedObject & object, const std::vector & path_points, + const geometry_msgs::msg::Point & current_pose, const double backward_distance, + const double forward_distance) +{ + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); + double max_dist_ego_to_obj = std::numeric_limits::lowest(); + const auto obj_polygon_outer = obj_polygon.outer(); + for (const auto & polygon_p : obj_polygon_outer) { + const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); + const double dist_ego_to_obj = + motion_utils::calcSignedArcLength(path_points, current_pose, obj_p); + max_dist_ego_to_obj = std::max(dist_ego_to_obj, max_dist_ego_to_obj); + } + + return (backward_distance < max_dist_ego_to_obj && max_dist_ego_to_obj < forward_distance); +} + +bool velocity_filter( + const PredictedObject & object, double velocity_threshold, double max_velocity) +{ + const auto v_norm = std::hypot( + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y); + return (velocity_threshold < v_norm && v_norm < max_velocity); +} +} // namespace behavior_path_planner::utils::path_safety_checker::filters + namespace behavior_path_planner::utils::path_safety_checker { bool isCentroidWithinLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet) @@ -41,21 +73,21 @@ bool isPolygonOverlapLanelet(const PredictedObject & object, const lanelet::Cons PredictedObjects filterObjects( const std::shared_ptr & objects, const std::shared_ptr & route_handler, const lanelet::ConstLanelets & current_lanes, - const geometry_msgs::msg::Point & current_pose, - const std::shared_ptr & params) + const geometry_msgs::msg::Point & current_pose, const ObjectsFilteringParams & params) { // Guard if (objects->objects.empty()) { return PredictedObjects(); } - const double ignore_object_velocity_threshold = params->ignore_object_velocity_threshold; - const double object_check_forward_distance = params->object_check_forward_distance; - const double object_check_backward_distance = params->object_check_backward_distance; - const ObjectTypesToCheck & target_object_types = params->object_types_to_check; + const auto ignore_object_velocity_threshold = params.ignore_object_velocity_threshold; + const auto object_check_forward_distance = params.object_check_forward_distance; + const auto object_check_backward_distance = params.object_check_backward_distance; + const auto remove_above_threshold = params.remove_velocity_above_threshold; + const auto & target_object_types = params.object_types_to_check; PredictedObjects filtered_objects = - filterObjectsByVelocity(*objects, ignore_object_velocity_threshold, false); + filterObjectsByVelocity(*objects, ignore_object_velocity_threshold, remove_above_threshold); filterObjectsByClass(filtered_objects, target_object_types); @@ -63,8 +95,8 @@ PredictedObjects filterObjects( current_lanes, object_check_backward_distance, object_check_forward_distance); filterObjectsByPosition( - filtered_objects, path.points, current_pose, object_check_forward_distance, - object_check_backward_distance); + filtered_objects, path.points, current_pose, object_check_backward_distance, + object_check_forward_distance); return filtered_objects; } @@ -82,31 +114,26 @@ PredictedObjects filterObjectsByVelocity( PredictedObjects filterObjectsByVelocity( const PredictedObjects & objects, double velocity_threshold, double max_velocity) { - const auto velocity_filter = [&](const auto & object) { - const auto v_norm = std::hypot( - object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.kinematics.initial_twist_with_covariance.twist.linear.y); - return (velocity_threshold < v_norm && v_norm < max_velocity); + const auto filter = [&](const auto & object) { + return filters::velocity_filter(object, velocity_threshold, max_velocity); }; PredictedObjects filtered = objects; - filterObjects(filtered, velocity_filter); + filterObjects(filtered, filter); return filtered; } void filterObjectsByPosition( PredictedObjects & objects, const std::vector & path_points, - const geometry_msgs::msg::Point & current_pose, const double forward_distance, - const double backward_distance) + const geometry_msgs::msg::Point & current_pose, const double backward_distance, + const double forward_distance) { - const auto position_filter = [&](const auto & object) { - const double dist_ego_to_obj = motion_utils::calcSignedArcLength( - path_points, current_pose, object.kinematics.initial_pose_with_covariance.pose.position); - - return (-backward_distance < dist_ego_to_obj && dist_ego_to_obj < forward_distance); + const auto filter = [&](const auto & object) { + return filters::position_filter( + object, path_points, current_pose, -backward_distance, forward_distance); }; - filterObjects(objects, position_filter); + filterObjects(objects, filter); } void filterObjectsByClass(