From 25e7d5db5acbf2faebce1779d4e6308df1d4104c Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 22 Nov 2024 16:30:22 +0900 Subject: [PATCH] refactor(lane_change): separate target lane leading based on obj behavior (#9372) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * separate target lane leading objects based on behavior (RT1-8532) Signed-off-by: Zulfaqar Azmi * fixed overlapped filtering and lanes debug marker Signed-off-by: Zulfaqar Azmi * combine filteredObjects function Signed-off-by: Zulfaqar Azmi * renaming functions and type Signed-off-by: Zulfaqar Azmi * update some logic to check is stopped Signed-off-by: Zulfaqar Azmi * rename expanded to stopped_outside_boundary Signed-off-by: Zulfaqar Azmi * Include docstring Signed-off-by: Zulfaqar Azmi * rename stopped_outside_boundary → stopped_at_bound Signed-off-by: Zulfaqar Azmi * Update planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> Signed-off-by: Zulfaqar Azmi * Update planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> Signed-off-by: Zulfaqar Azmi * spell-check Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> --- .../scene.hpp | 7 +- .../utils/base_class.hpp | 2 +- .../utils/data_structs.hpp | 34 ++- .../utils/debug_structs.hpp | 8 +- .../utils/markers.hpp | 4 +- .../utils/utils.hpp | 46 ++- .../package.xml | 1 + .../src/scene.cpp | 275 ++++++------------ .../src/utils/markers.cpp | 52 ++-- .../src/utils/utils.cpp | 139 +++++---- .../path_safety_checker/objects_filtering.hpp | 5 +- .../path_safety_checker/objects_filtering.cpp | 9 +- .../test/test_objects_filtering.cpp | 15 +- 13 files changed, 284 insertions(+), 313 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index dd21233c716f1..550925c4a10fc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -121,16 +121,13 @@ class NormalLaneChange : public LaneChangeBase TurnSignalInfo get_terminal_turn_signal_info() const final; lane_change::TargetObjects get_target_objects( - const FilteredByLanesExtendedObjects & filtered_objects, + const FilteredLanesObjects & filtered_objects, const lanelet::ConstLanelets & current_lanes) const; - FilteredByLanesExtendedObjects filterObjects() const; + FilteredLanesObjects filter_objects() const; void filterOncomingObjects(PredictedObjects & objects) const; - FilteredByLanesObjects filterObjectsByLanelets( - const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path) const; - bool get_prepare_segment( PathWithLaneId & prepare_segment, const double prepare_length) const override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp index 634f3b737942b..e5c7fcb6d621e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp @@ -276,7 +276,7 @@ class LaneChangeBase std::shared_ptr abort_path_{}; std::shared_ptr planner_data_{}; lane_change::CommonDataPtr common_data_ptr_{}; - FilteredByLanesExtendedObjects filtered_objects_{}; + FilteredLanesObjects filtered_objects_{}; BehaviorModuleOutput prev_module_output_{}; std::optional lane_change_stop_pose_{std::nullopt}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index 16b2b69a81af8..231353e812dd1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -262,25 +262,28 @@ struct Info } }; -template -struct LanesObjects +struct TargetLaneLeadingObjects { - Object current_lane{}; - Object target_lane_leading{}; - Object target_lane_trailing{}; - Object other_lane{}; - - LanesObjects() = default; - LanesObjects( - Object current_lane, Object target_lane_leading, Object target_lane_trailing, Object other_lane) - : current_lane(std::move(current_lane)), - target_lane_leading(std::move(target_lane_leading)), - target_lane_trailing(std::move(target_lane_trailing)), - other_lane(std::move(other_lane)) + ExtendedPredictedObjects moving; + ExtendedPredictedObjects stopped; + + // for objects outside of target lanes, but close to its boundaries + ExtendedPredictedObjects stopped_at_bound; + + [[nodiscard]] size_t size() const { + return moving.size() + stopped.size() + stopped_at_bound.size(); } }; +struct FilteredLanesObjects +{ + ExtendedPredictedObjects others; + ExtendedPredictedObjects current_lane; + ExtendedPredictedObjects target_lane_trailing; + TargetLaneLeadingObjects target_lane_leading; +}; + struct TargetObjects { ExtendedPredictedObjects leading; @@ -418,8 +421,7 @@ using LaneChangeStates = lane_change::States; using LaneChangePhaseInfo = lane_change::PhaseInfo; using LaneChangePhaseMetrics = lane_change::PhaseMetrics; using LaneChangeInfo = lane_change::Info; -using FilteredByLanesObjects = lane_change::LanesObjects>; -using FilteredByLanesExtendedObjects = lane_change::LanesObjects; +using FilteredLanesObjects = lane_change::FilteredLanesObjects; using LateralAccelerationMap = lane_change::LateralAccelerationMap; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/debug_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/debug_structs.hpp index fc51a82a8a842..a28b8523a75c7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/debug_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/debug_structs.hpp @@ -35,7 +35,7 @@ struct Debug LaneChangePaths valid_paths; CollisionCheckDebugMap collision_check_objects; CollisionCheckDebugMap collision_check_objects_after_approval; - FilteredByLanesExtendedObjects filtered_objects; + FilteredLanesObjects filtered_objects; geometry_msgs::msg::Polygon execution_area; geometry_msgs::msg::Pose ego_pose; lanelet::ConstLanelets current_lanes; @@ -55,9 +55,11 @@ struct Debug collision_check_objects.clear(); collision_check_objects_after_approval.clear(); filtered_objects.current_lane.clear(); - filtered_objects.target_lane_leading.clear(); filtered_objects.target_lane_trailing.clear(); - filtered_objects.other_lane.clear(); + filtered_objects.target_lane_leading.moving.clear(); + filtered_objects.target_lane_leading.stopped.clear(); + filtered_objects.target_lane_leading.stopped_at_bound.clear(); + filtered_objects.others.clear(); execution_area.points.clear(); current_lanes.clear(); target_lanes.clear(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp index d403e7e1436c7..c95b388a2e4a4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp @@ -29,7 +29,7 @@ namespace marker_utils::lane_change_markers { -using autoware::behavior_path_planner::FilteredByLanesExtendedObjects; +using autoware::behavior_path_planner::FilteredLanesObjects; using autoware::behavior_path_planner::LaneChangePath; using autoware::behavior_path_planner::lane_change::Debug; using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects; @@ -40,7 +40,7 @@ MarkerArray createLaneChangingVirtualWallMarker( const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name, const rclcpp::Time & now, const std::string & ns); MarkerArray showFilteredObjects( - const FilteredByLanesExtendedObjects & filtered_objects, const std::string & ns); + const FilteredLanesObjects & filtered_objects, const std::string & ns); MarkerArray createExecutionArea(const geometry_msgs::msg::Polygon & execution_area); MarkerArray showExecutionInfo(const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose); MarkerArray createDebugMarkerArray( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index edf64ecdc49ee..a02a61d5e72b6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -54,6 +54,7 @@ using behavior_path_planner::lane_change::CommonDataPtr; using behavior_path_planner::lane_change::LanesPolygon; using behavior_path_planner::lane_change::ModuleType; using behavior_path_planner::lane_change::PathSafetyStatus; +using behavior_path_planner::lane_change::TargetLaneLeadingObjects; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; @@ -144,8 +145,7 @@ lanelet::BasicPolygon2d create_polygon( const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist); ExtendedPredictedObject transform( - const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters, - const LaneChangeParameters & lane_change_parameters); + const PredictedObject & object, const LaneChangeParameters & lane_change_parameters); bool is_collided_polygons_in_lanelet( const std::vector & collided_polygons, const lanelet::BasicPolygon2d & lanes_polygon); @@ -240,17 +240,14 @@ bool is_same_lane_with_prev_iteration( bool is_ahead_of_ego( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, - const PredictedObject & object); + const ExtendedPredictedObject & object); bool is_before_terminal( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, - const PredictedObject & object); + const ExtendedPredictedObject & object); double calc_angle_to_lanelet_segment(const lanelet::ConstLanelets & lanelets, const Pose & pose); -ExtendedPredictedObjects transform_to_extended_objects( - const CommonDataPtr & common_data_ptr, const std::vector & objects); - double get_distance_to_next_regulatory_element( const CommonDataPtr & common_data_ptr, const bool ignore_crosswalk = false, const bool ignore_intersection = false); @@ -274,7 +271,7 @@ double get_distance_to_next_regulatory_element( * found, returns the maximum possible double value. */ double get_min_dist_to_current_lanes_obj( - const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects, + const CommonDataPtr & common_data_ptr, const FilteredLanesObjects & filtered_objects, const double dist_to_target_lane_start, const PathWithLaneId & path); /** @@ -294,8 +291,8 @@ double get_min_dist_to_current_lanes_obj( * otherwise, false. */ bool has_blocking_target_object( - const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects, - const double stop_arc_length, const PathWithLaneId & path); + const TargetLaneLeadingObjects & target_leading_objects, const double stop_arc_length, + const PathWithLaneId & path); /** * @brief Checks if the ego vehicle has passed any turn direction within an intersection. @@ -342,5 +339,34 @@ std::vector get_line_string_paths(const ExtendedPredictedObject & */ bool has_overtaking_turn_lane_object( const CommonDataPtr & common_data_ptr, const ExtendedPredictedObjects & trailing_objects); + +/** + * @brief Filters objects based on their positions and velocities relative to the ego vehicle and + * the target lane. + * + * This function evaluates whether an object should be classified as a leading or trailing object + * in the context of a lane change. Objects are filtered based on their lateral distance from + * the ego vehicle, velocity, and whether they are within the target lane or its expanded + * boundaries. + * + * @param common_data_ptr Shared pointer to CommonData containing information about current lanes, + * vehicle dimensions, lane polygons, and behavior parameters. + * @param object An extended predicted object representing a potential obstacle in the environment. + * @param dist_ego_to_current_lanes_center Distance from the ego vehicle to the center of the + * current lanes. + * @param ahead_of_ego Boolean flag indicating if the object is ahead of the ego vehicle. + * @param before_terminal Boolean flag indicating if the ego vehicle is before the terminal point of + * the lane. + * @param leading_objects Reference to a structure for storing leading objects (stopped, moving, or + * outside boundaries). + * @param trailing_objects Reference to a collection for storing trailing objects. + * + * @return true if the object is classified as either leading or trailing, false otherwise. + */ +bool filter_target_lane_objects( + const CommonDataPtr & common_data_ptr, const ExtendedPredictedObject & object, + const double dist_ego_to_current_lanes_center, const bool ahead_of_ego, + const bool before_terminal, TargetLaneLeadingObjects & leading_objects, + ExtendedPredictedObjects & trailing_objects); } // namespace autoware::behavior_path_planner::utils::lane_change #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml index a755b8dc0d42b..aef3d357c20da 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml @@ -27,6 +27,7 @@ autoware_rtc_interface autoware_universe_utils pluginlib + range-v3 rclcpp tier4_planning_msgs visualization_msgs diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 3f06ff0481c7e..942902b836c95 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -30,6 +30,10 @@ #include #include #include +#include +#include +#include +#include #include #include @@ -45,8 +49,8 @@ namespace autoware::behavior_path_planner { using autoware::motion_utils::calcSignedArcLength; using utils::lane_change::create_lanes_polygon; -using utils::path_safety_checker::isPolygonOverlapLanelet; namespace calculation = utils::lane_change::calculation; +using utils::path_safety_checker::filter::velocity_filter; NormalLaneChange::NormalLaneChange( const std::shared_ptr & parameters, LaneChangeModuleType type, @@ -111,6 +115,18 @@ void NormalLaneChange::update_lanes(const bool is_approved) *route_handler_ptr, get_target_lanes(), common_data_ptr_->get_ego_pose(), common_data_ptr_->lc_param_ptr->backward_lane_length); + lane_change_debug_.current_lanes = common_data_ptr_->lanes_ptr->current; + lane_change_debug_.target_lanes = common_data_ptr_->lanes_ptr->target; + + lane_change_debug_.target_backward_lanes.clear(); + ranges::for_each( + common_data_ptr_->lanes_ptr->preceding_target, + [&](const lanelet::ConstLanelets & preceding_lanes) { + ranges::insert( + lane_change_debug_.target_backward_lanes, lane_change_debug_.target_backward_lanes.end(), + preceding_lanes); + }); + *common_data_ptr_->lanes_polygon_ptr = create_lanes_polygon(common_data_ptr_); } @@ -193,7 +209,7 @@ void NormalLaneChange::update_transient_data() void NormalLaneChange::update_filtered_objects() { - filtered_objects_ = filterObjects(); + filtered_objects_ = filter_objects(); } void NormalLaneChange::updateLaneChangeStatus() @@ -566,7 +582,7 @@ void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path) // [ego]> | <--- stop margin ---> [obj]> // ---------------------------------------------------------- const auto has_blocking_target_lane_obj = utils::lane_change::has_blocking_target_object( - common_data_ptr_, filtered_objects_, stop_arc_length_behind_obj, path); + filtered_objects_.target_lane_leading, stop_arc_length_behind_obj, path); if (has_blocking_target_lane_obj || stop_arc_length_behind_obj <= 0.0) { set_stop_pose(dist_to_terminal_stop, path); @@ -926,30 +942,31 @@ bool NormalLaneChange::get_prepare_segment( } lane_change::TargetObjects NormalLaneChange::get_target_objects( - const FilteredByLanesExtendedObjects & filtered_objects, + const FilteredLanesObjects & filtered_objects, [[maybe_unused]] const lanelet::ConstLanelets & current_lanes) const { - ExtendedPredictedObjects leading_objects = filtered_objects.target_lane_leading; + ExtendedPredictedObjects leading_objects = filtered_objects.target_lane_leading.moving; + auto insert_leading_objects = [&](const auto & objects) { + ranges::actions::insert(leading_objects, leading_objects.end(), objects); + }; + + insert_leading_objects(filtered_objects.target_lane_leading.stopped); + insert_leading_objects(filtered_objects.target_lane_leading.stopped_at_bound); const auto chk_obj_in_curr_lanes = lane_change_parameters_->check_objects_on_current_lanes; if (chk_obj_in_curr_lanes || common_data_ptr_->transient_data.is_ego_stuck) { - leading_objects.insert( - leading_objects.end(), filtered_objects.current_lane.begin(), - filtered_objects.current_lane.end()); + insert_leading_objects(filtered_objects.current_lane); } const auto chk_obj_in_other_lanes = lane_change_parameters_->check_objects_on_other_lanes; if (chk_obj_in_other_lanes) { - leading_objects.insert( - leading_objects.end(), filtered_objects.other_lane.begin(), - filtered_objects.other_lane.end()); + insert_leading_objects(filtered_objects.others); } return {leading_objects, filtered_objects.target_lane_trailing}; } -FilteredByLanesExtendedObjects NormalLaneChange::filterObjects() const +FilteredLanesObjects NormalLaneChange::filter_objects() const { - const auto & route_handler = getRouteHandler(); auto objects = *planner_data_->dynamic_object; utils::path_safety_checker::filterObjectsByClass( objects, lane_change_parameters_->object_types_to_check); @@ -964,64 +981,81 @@ FilteredByLanesExtendedObjects NormalLaneChange::filterObjects() const return {}; } - const auto & current_lanes = get_current_lanes(); - - if (current_lanes.empty()) { + if (!common_data_ptr_->is_lanes_available()) { return {}; } - const auto & target_lanes = get_target_lanes(); + const auto & current_pose = common_data_ptr_->get_ego_pose(); + const auto & current_lanes = common_data_ptr_->lanes_ptr->current; - if (target_lanes.empty()) { - return {}; - } + const auto & current_lanes_ref_path = common_data_ptr_->current_lanes_path; - const auto & path = common_data_ptr_->current_lanes_path; + const auto & lanes_polygon = *common_data_ptr_->lanes_polygon_ptr; + const auto dist_ego_to_current_lanes_center = + lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); - auto filtered_by_lanes_objects = filterObjectsByLanelets(objects, path); + FilteredLanesObjects filtered_objects; + const auto reserve_size = objects.objects.size(); + filtered_objects.current_lane.reserve(reserve_size); + auto & target_lane_leading = filtered_objects.target_lane_leading; + target_lane_leading.stopped.reserve(reserve_size); + target_lane_leading.moving.reserve(reserve_size); + target_lane_leading.stopped_at_bound.reserve(reserve_size); + filtered_objects.target_lane_trailing.reserve(reserve_size); + filtered_objects.others.reserve(reserve_size); - const auto is_within_vel_th = [](const auto & object) -> bool { - constexpr double min_vel_th = 1.0; - constexpr double max_vel_th = std::numeric_limits::max(); - return utils::path_safety_checker::filter::velocity_filter(object, min_vel_th, max_vel_th); - }; + const auto stopped_obj_vel_th = common_data_ptr_->lc_param_ptr->stopped_object_velocity_threshold; - utils::path_safety_checker::filterObjects( - filtered_by_lanes_objects.target_lane_trailing, - [&](const PredictedObject & object) { return is_within_vel_th(object); }); + for (const auto & object : objects.objects) { + auto ext_object = utils::lane_change::transform(object, *common_data_ptr_->lc_param_ptr); + const auto & ext_obj_pose = ext_object.initial_pose; + ext_object.dist_from_ego = autoware::motion_utils::calcSignedArcLength( + current_lanes_ref_path.points, current_pose.position, ext_obj_pose.position); - if (lane_change_parameters_->check_objects_on_other_lanes) { - utils::path_safety_checker::filterObjects( - filtered_by_lanes_objects.other_lane, [&](const PredictedObject & object) { - const auto ahead_of_ego = - utils::lane_change::is_ahead_of_ego(common_data_ptr_, path, object); - return is_within_vel_th(object) && ahead_of_ego; - }); + const auto is_before_terminal = + utils::lane_change::is_before_terminal(common_data_ptr_, current_lanes_ref_path, ext_object); + + const auto ahead_of_ego = + utils::lane_change::is_ahead_of_ego(common_data_ptr_, current_lanes_ref_path, ext_object); + + if (utils::lane_change::filter_target_lane_objects( + common_data_ptr_, ext_object, dist_ego_to_current_lanes_center, ahead_of_ego, + is_before_terminal, target_lane_leading, filtered_objects.target_lane_trailing)) { + continue; + } + + // TODO(Azu): We have to think about how to remove is_within_vel_th without breaking AW behavior + const auto is_moving = velocity_filter( + ext_object.initial_twist, stopped_obj_vel_th, std::numeric_limits::max()); + + if ( + ahead_of_ego && is_moving && is_before_terminal && + !boost::geometry::disjoint(ext_object.initial_polygon, lanes_polygon.current)) { + // check only the objects that are in front of the ego vehicle + filtered_objects.current_lane.push_back(ext_object); + continue; + } + + filtered_objects.others.push_back(ext_object); } - // TODO(Azu): We have to think about how to remove is_within_vel_th without breaking AW behavior - utils::path_safety_checker::filterObjects( - filtered_by_lanes_objects.current_lane, [&](const PredictedObject & object) { - const auto ahead_of_ego = utils::lane_change::is_ahead_of_ego(common_data_ptr_, path, object); - return is_within_vel_th(object) && ahead_of_ego; - }); + const auto dist_comparator = [](const auto & obj1, const auto & obj2) { + return obj1.dist_from_ego < obj2.dist_from_ego; + }; + + // There are no use cases for other lane objects yet, so to save some computation time, we dont + // have to sort them. + ranges::sort(filtered_objects.current_lane, dist_comparator); + ranges::sort(target_lane_leading.stopped_at_bound, dist_comparator); + ranges::sort(target_lane_leading.stopped, dist_comparator); + ranges::sort(target_lane_leading.moving, dist_comparator); + ranges::sort(filtered_objects.target_lane_trailing, [&](const auto & obj1, const auto & obj2) { + return !dist_comparator(obj1, obj2); + }); + + lane_change_debug_.filtered_objects = filtered_objects; - const auto target_lane_leading_extended_objects = - utils::lane_change::transform_to_extended_objects( - common_data_ptr_, filtered_by_lanes_objects.target_lane_leading); - const auto target_lane_trailing_extended_objects = - utils::lane_change::transform_to_extended_objects( - common_data_ptr_, filtered_by_lanes_objects.target_lane_trailing); - const auto current_lane_extended_objects = utils::lane_change::transform_to_extended_objects( - common_data_ptr_, filtered_by_lanes_objects.current_lane); - const auto other_lane_extended_objects = utils::lane_change::transform_to_extended_objects( - common_data_ptr_, filtered_by_lanes_objects.other_lane); - - FilteredByLanesExtendedObjects lane_change_target_objects( - current_lane_extended_objects, target_lane_leading_extended_objects, - target_lane_trailing_extended_objects, other_lane_extended_objects); - lane_change_debug_.filtered_objects = lane_change_target_objects; - return lane_change_target_objects; + return filtered_objects; } void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const @@ -1038,7 +1072,8 @@ void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const const auto is_stopped_object = [](const auto & object) -> bool { constexpr double min_vel_th = -0.5; constexpr double max_vel_th = 0.5; - return utils::path_safety_checker::filter::velocity_filter(object, min_vel_th, max_vel_th); + return velocity_filter( + object.kinematics.initial_twist_with_covariance.twist, min_vel_th, max_vel_th); }; utils::path_safety_checker::filterObjects(objects, [&](const PredictedObject & object) { @@ -1051,122 +1086,6 @@ void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const }); } -FilteredByLanesObjects NormalLaneChange::filterObjectsByLanelets( - const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path) const -{ - std::vector target_lane_leading_objects; - std::vector target_lane_trailing_objects; - std::vector current_lane_objects; - std::vector other_lane_objects; - - const auto & current_pose = getEgoPose(); - const auto & current_lanes = common_data_ptr_->lanes_ptr->current; - const auto & target_lanes = common_data_ptr_->lanes_ptr->target; - const auto & route_handler = getRouteHandler(); - const auto & common_parameters = planner_data_->parameters; - const auto check_optional_polygon = [](const auto & object, const auto & polygon) { - return !polygon.empty() && isPolygonOverlapLanelet(object, polygon); - }; - - // get backward lanes - const auto & target_backward_lanes = common_data_ptr_->lanes_ptr->preceding_target; - - { - lane_change_debug_.current_lanes = current_lanes; - lane_change_debug_.target_lanes = target_lanes; - - // TODO(Azu) change the type to std::vector - lane_change_debug_.target_backward_lanes.clear(); - std::for_each( - target_backward_lanes.begin(), target_backward_lanes.end(), - [&](const lanelet::ConstLanelets & target_backward_lane) { - lane_change_debug_.target_backward_lanes.insert( - lane_change_debug_.target_backward_lanes.end(), target_backward_lane.begin(), - target_backward_lane.end()); - }); - } - - const auto & lanes_polygon = *common_data_ptr_->lanes_polygon_ptr; - const auto dist_ego_to_current_lanes_center = - lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); - - const auto reserve_size = objects.objects.size(); - current_lane_objects.reserve(reserve_size); - target_lane_leading_objects.reserve(reserve_size); - target_lane_trailing_objects.reserve(reserve_size); - other_lane_objects.reserve(reserve_size); - - for (const auto & object : objects.objects) { - const auto is_lateral_far = std::invoke([&]() -> bool { - 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); - }); - - const auto is_before_terminal = [&]() { - return utils::lane_change::is_before_terminal( - common_data_ptr_, current_lanes_ref_path, object); - }; - - if ( - check_optional_polygon(object, lanes_polygon.target) && is_lateral_far && - is_before_terminal()) { - const auto ahead_of_ego = - utils::lane_change::is_ahead_of_ego(common_data_ptr_, current_lanes_ref_path, object); - if (ahead_of_ego) { - target_lane_leading_objects.push_back(object); - } else { - target_lane_trailing_objects.push_back(object); - } - continue; - } - - if ( - check_optional_polygon(object, lanes_polygon.expanded_target) && is_lateral_far && - is_before_terminal()) { - const auto ahead_of_ego = - utils::lane_change::is_ahead_of_ego(common_data_ptr_, current_lanes_ref_path, object); - const auto stopped_obj_vel_th = - common_data_ptr_->lc_param_ptr->stopped_object_velocity_threshold; - if (object.kinematics.initial_twist_with_covariance.twist.linear.x < stopped_obj_vel_th) { - if (ahead_of_ego) { - target_lane_leading_objects.push_back(object); - continue; - } - } - } - - const auto is_overlap_target_backward = std::invoke([&]() -> bool { - const auto check_backward_polygon = [&object](const auto & target_backward_polygon) { - return isPolygonOverlapLanelet(object, target_backward_polygon); - }; - return std::any_of( - lanes_polygon.preceding_target.begin(), lanes_polygon.preceding_target.end(), - check_backward_polygon); - }); - - // check if the object intersects with target backward lanes - if (is_overlap_target_backward) { - target_lane_trailing_objects.push_back(object); - continue; - } - - if (check_optional_polygon(object, lanes_polygon.current)) { - // check only the objects that are in front of the ego vehicle - current_lane_objects.push_back(object); - continue; - } - - other_lane_objects.push_back(object); - } - - return { - current_lane_objects, target_lane_leading_objects, target_lane_trailing_objects, - other_lane_objects}; -} - PathWithLaneId NormalLaneChange::getTargetSegment( const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, const double target_lane_length, const double lane_changing_length, @@ -1420,7 +1339,7 @@ bool NormalLaneChange::check_candidate_path_safety( if ( !is_stuck && !utils::lane_change::passed_parked_objects( - common_data_ptr_, candidate_path, filtered_objects_.target_lane_leading, + common_data_ptr_, candidate_path, filtered_objects_.target_lane_leading.stopped, lane_change_debug_.collision_check_objects)) { throw std::logic_error( "Ego is not stuck and parked vehicle exists in the target lane. Skip lane change."); @@ -1601,7 +1520,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const } const auto has_passed_parked_objects = utils::lane_change::passed_parked_objects( - common_data_ptr_, path, filtered_objects_.target_lane_leading, debug_data); + common_data_ptr_, path, filtered_objects_.target_lane_leading.stopped, debug_data); if (!has_passed_parked_objects) { RCLCPP_DEBUG(logger_, "Lane change has been delayed."); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp index 7aea478bd6794..5700842780395 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp @@ -101,36 +101,34 @@ MarkerArray createLaneChangingVirtualWallMarker( } MarkerArray showFilteredObjects( - const FilteredByLanesExtendedObjects & filtered_objects, const std::string & ns) + const FilteredLanesObjects & filtered_objects, const std::string & ns) { int32_t update_id = 0; - auto current_marker = marker_utils::showFilteredObjects( - filtered_objects.current_lane, ns, colors::yellow(), update_id); - update_id += static_cast(current_marker.markers.size()); - auto target_leading_marker = marker_utils::showFilteredObjects( - filtered_objects.target_lane_leading, ns, colors::aqua(), update_id); - update_id += static_cast(target_leading_marker.markers.size()); - auto target_trailing_marker = marker_utils::showFilteredObjects( - filtered_objects.target_lane_trailing, ns, colors::blue(), update_id); - update_id += static_cast(target_trailing_marker.markers.size()); - auto other_marker = marker_utils::showFilteredObjects( - filtered_objects.other_lane, ns, colors::medium_orchid(), update_id); - MarkerArray marker_array; - std::move( - current_marker.markers.begin(), current_marker.markers.end(), - std::back_inserter(marker_array.markers)); - std::move( - target_leading_marker.markers.begin(), target_leading_marker.markers.end(), - std::back_inserter(marker_array.markers)); - - std::move( - target_trailing_marker.markers.begin(), target_trailing_marker.markers.end(), - std::back_inserter(marker_array.markers)); - - std::move( - other_marker.markers.begin(), other_marker.markers.end(), - std::back_inserter(marker_array.markers)); + auto reserve_size = filtered_objects.current_lane.size() + filtered_objects.others.size() + + filtered_objects.target_lane_leading.size() + + filtered_objects.target_lane_trailing.size(); + marker_array.markers.reserve(2 * reserve_size); + auto add_objects_to_marker = + [&](const ExtendedPredictedObjects & objects, const ColorRGBA & color) { + if (objects.empty()) { + return; + } + + auto marker = marker_utils::showFilteredObjects(objects, ns, color, update_id); + update_id += static_cast(marker.markers.size()); + std::move( + marker.markers.begin(), marker.markers.end(), std::back_inserter(marker_array.markers)); + }; + + add_objects_to_marker(filtered_objects.current_lane, colors::yellow()); + add_objects_to_marker(filtered_objects.target_lane_leading.moving, colors::aqua()); + add_objects_to_marker(filtered_objects.target_lane_leading.stopped, colors::light_steel_blue()); + add_objects_to_marker(filtered_objects.target_lane_trailing, colors::blue()); + add_objects_to_marker( + filtered_objects.target_lane_leading.stopped_at_bound, colors::light_pink()); + add_objects_to_marker(filtered_objects.others, colors::medium_orchid()); + return marker_array; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 69cf6441ef766..49551aa73f635 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -25,6 +25,7 @@ #include "autoware/object_recognition_utils/predicted_path_utils.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" +#include #include #include #include @@ -33,6 +34,7 @@ #include #include #include +#include #include #include @@ -912,9 +914,7 @@ lanelet::BasicPolygon2d create_polygon( } ExtendedPredictedObject transform( - const PredictedObject & object, - [[maybe_unused]] const BehaviorPathPlannerParameters & common_parameters, - const LaneChangeParameters & lane_change_parameters) + const PredictedObject & object, const LaneChangeParameters & lane_change_parameters) { ExtendedPredictedObject extended_object(object); @@ -1078,21 +1078,15 @@ bool is_same_lane_with_prev_iteration( bool is_ahead_of_ego( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, - const PredictedObject & object) + const ExtendedPredictedObject & object) { - const auto & current_ego_pose = common_data_ptr->get_ego_pose(); - - const auto & obj_position = object.kinematics.initial_pose_with_covariance.pose.position; - - const auto dist_to_base_link = autoware::motion_utils::calcSignedArcLength( - path.points, current_ego_pose.position, obj_position); const auto & ego_info = common_data_ptr->bpp_param_ptr->vehicle_info; const auto lon_dev = std::max( ego_info.max_longitudinal_offset_m + ego_info.rear_overhang_m, object.shape.dimensions.x); // we don't always have to check the distance accurately. - if (std::abs(dist_to_base_link) > lon_dev) { - return dist_to_base_link >= 0.0; + if (std::abs(object.dist_from_ego) > lon_dev) { + return object.dist_from_ego >= 0.0; } const auto & current_footprint = common_data_ptr->transient_data.current_footprint.outer(); @@ -1105,9 +1099,8 @@ bool is_ahead_of_ego( ego_min_dist_to_end = std::min(dist_to_end, ego_min_dist_to_end); } - const auto obj_polygon = autoware::universe_utils::toPolygon2d(object).outer(); auto current_min_dist_to_end = std::numeric_limits::max(); - for (const auto & polygon_p : obj_polygon) { + for (const auto & polygon_p : object.initial_polygon.outer()) { const auto obj_p = autoware::universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); const auto dist_ego_to_obj = autoware::motion_utils::calcSignedArcLength( path.points, obj_p, path.points.back().point.pose.position); @@ -1118,7 +1111,7 @@ bool is_ahead_of_ego( bool is_before_terminal( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, - const PredictedObject & object) + const ExtendedPredictedObject & object) { const auto & route_handler_ptr = common_data_ptr->route_handler_ptr; const auto & lanes_ptr = common_data_ptr->lanes_ptr; @@ -1127,7 +1120,7 @@ bool is_before_terminal( : path.points.back().point.pose.position; double current_max_dist = std::numeric_limits::lowest(); - const auto & obj_position = object.kinematics.initial_pose_with_covariance.pose.position; + const auto & obj_position = object.initial_pose.position; const auto dist_to_base_link = autoware::motion_utils::calcSignedArcLength(path.points, obj_position, terminal_position); // we don't always have to check the distance accurately. @@ -1135,8 +1128,7 @@ bool is_before_terminal( return dist_to_base_link >= 0.0; } - const auto obj_polygon = autoware::universe_utils::toPolygon2d(object).outer(); - for (const auto & polygon_p : obj_polygon) { + for (const auto & polygon_p : object.initial_polygon.outer()) { const auto obj_p = autoware::universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); const auto dist_obj_to_terminal = autoware::motion_utils::calcSignedArcLength(path.points, obj_p, terminal_position); @@ -1156,22 +1148,6 @@ double calc_angle_to_lanelet_segment(const lanelet::ConstLanelets & lanelets, co return std::abs(autoware::universe_utils::calcYawDeviation(closest_pose, pose)); } -ExtendedPredictedObjects transform_to_extended_objects( - const CommonDataPtr & common_data_ptr, const std::vector & objects) -{ - ExtendedPredictedObjects extended_objects; - extended_objects.reserve(objects.size()); - - const auto & bpp_param = *common_data_ptr->bpp_param_ptr; - const auto & lc_param = *common_data_ptr->lc_param_ptr; - std::transform( - objects.begin(), objects.end(), std::back_inserter(extended_objects), [&](const auto & object) { - return utils::lane_change::transform(object, bpp_param, lc_param); - }); - - return extended_objects; -} - double get_distance_to_next_regulatory_element( const CommonDataPtr & common_data_ptr, const bool ignore_crosswalk, const bool ignore_intersection) @@ -1200,7 +1176,7 @@ double get_distance_to_next_regulatory_element( } double get_min_dist_to_current_lanes_obj( - const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects, + const CommonDataPtr & common_data_ptr, const FilteredLanesObjects & filtered_objects, const double dist_to_target_lane_start, const PathWithLaneId & path) { const auto & path_points = path.points; @@ -1240,28 +1216,15 @@ double get_min_dist_to_current_lanes_obj( } bool has_blocking_target_object( - const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects, - const double stop_arc_length, const PathWithLaneId & path) + const TargetLaneLeadingObjects & target_leading_objects, const double stop_arc_length, + const PathWithLaneId & path) { - return std::any_of( - filtered_objects.target_lane_leading.begin(), filtered_objects.target_lane_leading.end(), - [&](const auto & object) { - const auto v = std::abs(object.initial_twist.linear.x); - if (v > common_data_ptr->lc_param_ptr->stop_velocity_threshold) { - return false; - } - - // filtered_objects includes objects out of target lanes, so filter them out - if (boost::geometry::disjoint( - object.initial_polygon, common_data_ptr->lanes_polygon_ptr->target)) { - return false; - } - - const auto arc_length_to_target_lane_obj = motion_utils::calcSignedArcLength( - path.points, path.points.front().point.pose.position, object.initial_pose.position); - const auto width_margin = object.shape.dimensions.x / 2; - return (arc_length_to_target_lane_obj - width_margin) >= stop_arc_length; - }); + return ranges::any_of(target_leading_objects.stopped, [&](const auto & object) { + const auto arc_length_to_target_lane_obj = motion_utils::calcSignedArcLength( + path.points, path.points.front().point.pose.position, object.initial_pose.position); + const auto width_margin = object.shape.dimensions.x / 2; + return (arc_length_to_target_lane_obj - width_margin) >= stop_arc_length; + }); } bool has_passed_intersection_turn_direction(const CommonDataPtr & common_data_ptr) @@ -1323,4 +1286,68 @@ bool has_overtaking_turn_lane_object( return std::any_of( trailing_objects.begin(), trailing_objects.end(), is_object_overlap_with_target); } + +bool filter_target_lane_objects( + const CommonDataPtr & common_data_ptr, const ExtendedPredictedObject & object, + const double dist_ego_to_current_lanes_center, const bool ahead_of_ego, + const bool before_terminal, TargetLaneLeadingObjects & leading_objects, + ExtendedPredictedObjects & trailing_objects) +{ + using behavior_path_planner::utils::path_safety_checker::filter::velocity_filter; + const auto & current_lanes = common_data_ptr->lanes_ptr->current; + const auto & vehicle_width = common_data_ptr->bpp_param_ptr->vehicle_info.vehicle_width_m; + const auto & lanes_polygon = *common_data_ptr->lanes_polygon_ptr; + const auto stopped_obj_vel_th = common_data_ptr->lc_param_ptr->stopped_object_velocity_threshold; + + const auto is_lateral_far = std::invoke([&]() -> bool { + const auto dist_object_to_current_lanes_center = + lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, object.initial_pose); + const auto lateral = dist_object_to_current_lanes_center - dist_ego_to_current_lanes_center; + return std::abs(lateral) > (vehicle_width / 2); + }); + + const auto is_stopped = velocity_filter( + object.initial_twist, -std::numeric_limits::epsilon(), stopped_obj_vel_th); + if (is_lateral_far && before_terminal) { + const auto in_target_lanes = + !boost::geometry::disjoint(object.initial_polygon, lanes_polygon.target); + if (in_target_lanes) { + if (!ahead_of_ego && !is_stopped) { + trailing_objects.push_back(object); + return true; + } + + if (ahead_of_ego) { + if (is_stopped) { + leading_objects.stopped.push_back(object); + } else { + leading_objects.moving.push_back(object); + } + return true; + } + } + + // Check if the object is positioned outside the lane boundary but still close to its edge. + const auto in_expanded_target_lanes = + !boost::geometry::disjoint(object.initial_polygon, lanes_polygon.expanded_target); + + if (in_expanded_target_lanes && is_stopped && ahead_of_ego) { + leading_objects.stopped_at_bound.push_back(object); + return true; + } + } + + const auto is_overlap_target_backward = + ranges::any_of(lanes_polygon.preceding_target, [&](const auto & target_backward_polygon) { + return !boost::geometry::disjoint(object.initial_polygon, target_backward_polygon); + }); + + // check if the object intersects with target backward lanes + if (is_overlap_target_backward && !is_stopped) { + trailing_objects.push_back(object); + return true; + } + + return false; +} } // namespace autoware::behavior_path_planner::utils::lane_change diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp index d9873975dabe1..5c29f347ad9ef 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp @@ -37,13 +37,12 @@ using tier4_planning_msgs::msg::PathPointWithLaneId; /** * @brief Filters object based on velocity. * - * @param object The predicted object to filter. + * @param twist The twist of predicted object to filter. * @param velocity_threshold Lower bound * @param max_velocity Upper bound * @return Returns true when the object is within a certain velocity range. */ -bool velocity_filter( - const PredictedObject & object, double velocity_threshold, double max_velocity); +bool velocity_filter(const Twist & object_twist, double velocity_threshold, double max_velocity); /** * @brief Filters object based on position. diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index 80936b655cc48..3c1d9f194aeec 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -26,11 +26,9 @@ namespace autoware::behavior_path_planner::utils::path_safety_checker::filter { -bool velocity_filter(const PredictedObject & object, double velocity_threshold, double max_velocity) +bool velocity_filter(const Twist & object_twist, 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); + const auto v_norm = std::hypot(object_twist.linear.x, object_twist.linear.y); return (velocity_threshold < v_norm && v_norm < max_velocity); } @@ -148,7 +146,8 @@ PredictedObjects filterObjectsByVelocity( const PredictedObjects & objects, double velocity_threshold, double max_velocity) { const auto filter = [&](const auto & object) { - return filter::velocity_filter(object, velocity_threshold, max_velocity); + return filter::velocity_filter( + object.kinematics.initial_twist_with_covariance.twist, velocity_threshold, max_velocity); }; auto filtered = objects; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp index a2058db51c9ea..88ed968d27b64 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp @@ -66,13 +66,14 @@ TEST(BehaviorPathPlanningObjectsFiltering, velocity_filter) using autoware::behavior_path_planner::utils::path_safety_checker::filter::velocity_filter; PredictedObject predicted_obj; - predicted_obj.kinematics.initial_twist_with_covariance.twist.linear.x = 4.0; - predicted_obj.kinematics.initial_twist_with_covariance.twist.linear.y = 3.0; - - EXPECT_TRUE(velocity_filter(predicted_obj, 4.0, 10.0)); - EXPECT_FALSE(velocity_filter(predicted_obj, 6.0, 10.0)); - EXPECT_FALSE(velocity_filter(predicted_obj, 2.0, 4.9)); - EXPECT_FALSE(velocity_filter(predicted_obj, 6.0, 2.0)); + auto & twist = predicted_obj.kinematics.initial_twist_with_covariance.twist; + twist.linear.x = 4.0; + twist.linear.y = 3.0; + + EXPECT_TRUE(velocity_filter(twist, 4.0, 10.0)); + EXPECT_FALSE(velocity_filter(twist, 6.0, 10.0)); + EXPECT_FALSE(velocity_filter(twist, 2.0, 4.9)); + EXPECT_FALSE(velocity_filter(twist, 6.0, 2.0)); } TEST(BehaviorPathPlanningObjectsFiltering, position_filter)