diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 162dd855a577a..fac0e921c1072 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -31,6 +31,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/utils/utils.cpp src/utils/path_utils.cpp src/utils/path_safety_checker/safety_check.cpp + src/utils/path_safety_checker/objects_filtering.cpp src/utils/avoidance/utils.cpp src/utils/lane_change/utils.cpp src/utils/side_shift/util.cpp 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 948a2cc4fbb2a..9a6a12427d783 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 @@ -26,10 +26,10 @@ namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathWithLaneId; -using behavior_path_planner::utils::safety_check::ExtendedPredictedObject; -using behavior_path_planner::utils::safety_check::PoseWithVelocityAndPolygonStamped; -using behavior_path_planner::utils::safety_check::PoseWithVelocityStamped; -using behavior_path_planner::utils::safety_check::PredictedPathWithPolygon; +using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index 27128e92ab971..6b7830147ebf9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -17,6 +17,7 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include @@ -28,10 +29,10 @@ namespace behavior_path_planner::utils::avoidance { using behavior_path_planner::PlannerData; -using behavior_path_planner::utils::safety_check::ExtendedPredictedObject; -using behavior_path_planner::utils::safety_check::PoseWithVelocityAndPolygonStamped; -using behavior_path_planner::utils::safety_check::PoseWithVelocityStamped; -using behavior_path_planner::utils::safety_check::PredictedPathWithPolygon; +using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; bool isOnRight(const ObjectData & obj); 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 7a3e96aa3fa3d..721c5b00f6e33 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 @@ -15,6 +15,7 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "lanelet2_core/geometry/Lanelet.h" @@ -132,9 +133,9 @@ struct LaneChangeTargetObjectIndices struct LaneChangeTargetObjects { - std::vector current_lane{}; - std::vector target_lane{}; - std::vector other_lane{}; + std::vector current_lane{}; + std::vector target_lane{}; + std::vector other_lane{}; }; enum class LaneChangeModuleType { 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 5fa22a8935bf2..f59e0170bdf14 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 @@ -19,6 +19,7 @@ #include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -43,10 +44,10 @@ 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::utils::safety_check::ExtendedPredictedObject; -using behavior_path_planner::utils::safety_check::PoseWithVelocityAndPolygonStamped; -using behavior_path_planner::utils::safety_check::PoseWithVelocityStamped; -using behavior_path_planner::utils::safety_check::PredictedPathWithPolygon; +using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; using data::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; 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 new file mode 100644 index 0000000000000..b4575eb4d3b7e --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp @@ -0,0 +1,179 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ + +#include "behavior_path_planner/data_manager.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +namespace behavior_path_planner::utils::path_safety_checker +{ + +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; + +/** + * @brief Filters objects based on various criteria. + * + * @param objects The predicted objects to filter. + * @param route_handler + * @param current_lanes + * @param current_pose The current pose of ego vehicle. + * @param params The filtering parameters. + * @return PredictedObjects The filtered objects. + */ +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); + +/** + * @brief Filter objects based on their velocity. + * + * @param objects The predicted objects to filter. + * @param lim_v Velocity limit for filtering. + * @return PredictedObjects The filtered objects. + */ +PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v); + +/** + * @brief Filter objects based on a velocity range. + * + * @param objects The predicted objects to filter. + * @param min_v Minimum velocity for filtering. + * @param max_v Maximum velocity for filtering. + * @return PredictedObjects The filtered objects. + */ +PredictedObjects filterObjectsByVelocity( + const PredictedObjects & objects, double min_v, double max_v); + +/** + * @brief Filter objects based on their position relative to a current_pose. + * + * @param objects The predicted objects to filter. + * @param path_points Points on the path. + * @param current_pose Current pose of the reference (e.g., ego vehicle). + * @param forward_distance Maximum forward distance for filtering. + * @param backward_distance Maximum backward distance for filtering. + */ +void filterObjectsByPosition( + PredictedObjects & objects, const std::vector & path_points, + const geometry_msgs::msg::Point & current_pose, const double forward_distance, + const double backward_distance); +/** + * @brief Filters the provided objects based on their classification. + * + * @param objects The predicted objects to be filtered. + * @param target_object_types The types of objects to retain after filtering. + */ +void filterObjectsByClass( + PredictedObjects & objects, const ObjectTypesToCheck & target_object_types); + +/** + * @brief Separate index of the obstacles into two part based on whether the object is within + * lanelet. + * @return Indices of objects pair. first objects are in the lanelet, and second others are out of + * lanelet. + */ +std::pair, std::vector> separateObjectIndicesByLanelets( + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); + +/** + * @brief Separate the objects into two part based on whether the object is within lanelet. + * @return Objects pair. first objects are in the lanelet, and second others are out of lanelet. + */ +std::pair separateObjectsByLanelets( + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); + +/** + * @brief Get the predicted path from an object. + * + * @param obj The extended predicted object. + * @param is_use_all_predicted_path Flag to determine whether to use all predicted paths or just the + * one with maximum confidence. + * @return std::vector The predicted path(s) from the object. + */ +std::vector getPredictedPathFromObj( + const ExtendedPredictedObject & obj, const bool & is_use_all_predicted_path); + +/** + * @brief Create a predicted path based on ego vehicle parameters. + * + * @param ego_predicted_path_params Parameters for ego's predicted path. + * @param path_points Points on the path. + * @param vehicle_pose Current pose of the ego-vehicle. + * @param current_velocity Current velocity of the vehicle. + * @param ego_seg_idx Index of the ego segment. + * @return std::vector The predicted path. + */ +std::vector createPredictedPath( + const std::shared_ptr & ego_predicted_path_params, + const std::vector & path_points, + const geometry_msgs::msg::Pose & vehicle_pose, const double current_velocity, size_t ego_seg_idx); + +/** + * @brief Checks if the centroid of a given object is within the provided lanelets. + * + * @param object The predicted object to check. + * @param target_lanelets The lanelets to check against. + * @return bool True if the object's centroid is within the lanelets, otherwise false. + */ +bool isCentroidWithinLanelets( + const PredictedObject & object, const lanelet::ConstLanelets & target_lanelets); + +/** + * @brief Transforms a given object into an extended predicted object. + * + * @param object The predicted object to transform. + * @param safety_check_time_horizon The time horizon for safety checks. + * @param safety_check_time_resolution The time resolution for safety checks. + * @return ExtendedPredictedObject The transformed object. + */ +ExtendedPredictedObject transform( + const PredictedObject & object, const double safety_check_time_horizon, + const double safety_check_time_resolution); + +/** + * @brief Creates target objects on a lane based on provided parameters. + * + * @param current_lanes + * @param route_handler + * @param filtered_objects The filtered objects. + * @param params The filtering parameters. + * @return TargetObjectsOnLane The target objects on the lane. + */ +TargetObjectsOnLane createTargetObjectsOnLane( + const lanelet::ConstLanelets & current_lanes, const std::shared_ptr & route_handler, + const PredictedObjects & filtered_objects, + const std::shared_ptr & params); + +} // namespace behavior_path_planner::utils::path_safety_checker + +#endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ 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 new file mode 100644 index 0000000000000..519f589f561ee --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -0,0 +1,174 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ + +#include + +#include +#include + +#include + +namespace behavior_path_planner::utils::path_safety_checker +{ + +using geometry_msgs::msg::Pose; +using tier4_autoware_utils::Polygon2d; + +struct PoseWithVelocity +{ + Pose pose; + double velocity{0.0}; + + PoseWithVelocity(const Pose & pose, const double velocity) : pose(pose), velocity(velocity) {} +}; + +struct PoseWithVelocityStamped : public PoseWithVelocity +{ + double time{0.0}; + + PoseWithVelocityStamped(const double time, const Pose & pose, const double velocity) + : PoseWithVelocity(pose, velocity), time(time) + { + } +}; + +struct PoseWithVelocityAndPolygonStamped : public PoseWithVelocityStamped +{ + Polygon2d poly; + + PoseWithVelocityAndPolygonStamped( + const double time, const Pose & pose, const double velocity, const Polygon2d & poly) + : PoseWithVelocityStamped(time, pose, velocity), poly(poly) + { + } +}; + +struct PredictedPathWithPolygon +{ + float confidence{0.0}; + std::vector path; +}; + +struct ExtendedPredictedObject +{ + unique_identifier_msgs::msg::UUID uuid; + geometry_msgs::msg::PoseWithCovariance initial_pose; + geometry_msgs::msg::TwistWithCovariance initial_twist; + geometry_msgs::msg::AccelWithCovariance initial_acceleration; + autoware_auto_perception_msgs::msg::Shape shape; + std::vector predicted_paths; +}; + +/** + * @brief Specifies which object class should be checked. + */ +struct ObjectTypesToCheck +{ + bool check_car{true}; ///< Check for cars. + bool check_truck{true}; ///< Check for trucks. + bool check_bus{true}; ///< Check for buses. + bool check_trailer{true}; ///< Check for trailers. + bool check_unknown{true}; ///< Check for unknown object types. + bool check_bicycle{true}; ///< Check for bicycles. + bool check_motorcycle{true}; ///< Check for motorcycles. + bool check_pedestrian{true}; ///< Check for pedestrians. +}; + +/** + * @brief Configuration for which lanes should be checked for objects. + */ +struct ObjectLaneConfiguration +{ + bool check_current_lane{}; ///< Check the current lane. + bool check_right_lane{}; ///< Check the right lane. + bool check_left_lane{}; ///< Check the left lane. + bool check_shoulder_lane{}; ///< Check the shoulder lane. + bool check_other_lane{}; ///< Check other lanes. +}; + +/** + * @brief Contains objects on lanes type. + */ +struct TargetObjectsOnLane +{ + std::vector on_current_lane{}; ///< Objects on the current lane. + std::vector on_right_lane{}; ///< Objects on the right lane. + std::vector on_left_lane{}; ///< Objects on the left lane. + std::vector on_shoulder_lane{}; ///< Objects on the shoulder lane. + std::vector on_other_lane{}; ///< Objects on other lanes. +}; + +/** + * @brief Parameters related to the RSS (Responsibility-Sensitive Safety) model. + */ +struct RSSparams +{ + double rear_vehicle_reaction_time{0.0}; ///< Reaction time of the rear vehicle. + double rear_vehicle_safety_time_margin{0.0}; ///< Safety time margin for the rear vehicle. + double lateral_distance_max_threshold{0.0}; ///< Maximum threshold for lateral distance. + double longitudinal_distance_min_threshold{ + 0.0}; ///< Minimum threshold for longitudinal distance. + double longitudinal_velocity_delta_time{0.0}; ///< Delta time for longitudinal velocity. +}; + +/** + * @brief Parameters for generating the ego vehicle's predicted path. + */ +struct EgoPredictedPathParams +{ + double acceleration; ///< Acceleration value. + double time_horizon; ///< Time horizon for prediction. + double time_resolution; ///< Time resolution for prediction. + double min_slow_speed; ///< Minimum slow speed. + double delay_until_departure; ///< Delay before departure. + double target_velocity; ///< Target velocity. +}; + +/** + * @brief Parameters for filtering objects. + */ +struct ObjectsFilteringParams +{ + double safety_check_time_horizon; ///< Time horizon for object's prediction. + double safety_check_time_resolution; ///< Time resolution for object's prediction. + double object_check_forward_distance; ///< Forward distance for object checks. + double object_check_backward_distance; ///< Backward distance for object checks. + double ignore_object_velocity_threshold; ///< 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; ///< Include the opposite lane in checks. + bool invert_opposite_lane; ///< Invert the opposite lane in checks. + bool check_all_predicted_path; ///< Check all predicted paths. + bool use_all_predicted_path; ///< Use all predicted paths. + bool use_predicted_path_outside_lanelet; ///< Use predicted paths outside of lanelets. +}; + +/** + * @brief Parameters for safety checks. + */ +struct SafetyCheckParams +{ + bool enable_safety_check; ///< Enable safety checks. + double backward_lane_length; ///< Length of the backward lane for path generation. + double forward_path_length; ///< Length of the forward path lane for path generation. + RSSparams rss_params; ///< Parameters related to the RSS model. + bool publish_debug_marker{false}; ///< Option to publish debug markers. +}; + +} // namespace behavior_path_planner::utils::path_safety_checker + +#endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index 55bac83442e84..edfec6a7db579 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -17,6 +17,7 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/marker_utils/utils.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include @@ -36,7 +37,7 @@ #include #include -namespace behavior_path_planner::utils::safety_check +namespace behavior_path_planner::utils::path_safety_checker { using autoware_auto_perception_msgs::msg::PredictedObject; @@ -49,51 +50,6 @@ using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; using vehicle_info_util::VehicleInfo; -struct PoseWithVelocity -{ - Pose pose; - double velocity{0.0}; - - PoseWithVelocity(const Pose & pose, const double velocity) : pose(pose), velocity(velocity) {} -}; - -struct PoseWithVelocityStamped : public PoseWithVelocity -{ - double time{0.0}; - - PoseWithVelocityStamped(const double time, const Pose & pose, const double velocity) - : PoseWithVelocity(pose, velocity), time(time) - { - } -}; - -struct PoseWithVelocityAndPolygonStamped : public PoseWithVelocityStamped -{ - Polygon2d poly; - - PoseWithVelocityAndPolygonStamped( - const double time, const Pose & pose, const double velocity, const Polygon2d & poly) - : PoseWithVelocityStamped(time, pose, velocity), poly(poly) - { - } -}; - -struct PredictedPathWithPolygon -{ - float confidence{0.0}; - std::vector path; -}; - -struct ExtendedPredictedObject -{ - unique_identifier_msgs::msg::UUID uuid; - geometry_msgs::msg::PoseWithCovariance initial_pose; - geometry_msgs::msg::TwistWithCovariance initial_twist; - geometry_msgs::msg::AccelWithCovariance initial_acceleration; - autoware_auto_perception_msgs::msg::Shape shape; - std::vector predicted_paths; -}; - namespace bg = boost::geometry; bool isTargetObjectFront( @@ -145,6 +101,6 @@ bool checkCollision( const BehaviorPathPlannerParameters & common_parameters, const double front_object_deceleration, const double rear_object_deceleration, CollisionCheckDebug & debug); -} // namespace behavior_path_planner::utils::safety_check +} // namespace behavior_path_planner::utils::path_safety_checker #endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index 088a0f0bd795b..784258e31b265 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -19,6 +19,7 @@ #include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/utils/lane_following/module_data.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include "motion_utils/motion_utils.hpp" @@ -66,16 +67,19 @@ using autoware_auto_perception_msgs::msg::Shape; using autoware_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; -using behavior_path_planner::utils::safety_check::ExtendedPredictedObject; -using behavior_path_planner::utils::safety_check::PoseWithVelocityAndPolygonStamped; -using behavior_path_planner::utils::safety_check::PoseWithVelocityStamped; -using behavior_path_planner::utils::safety_check::PredictedPathWithPolygon; using drivable_area_expansion::DrivableAreaExpansionParameters; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseArray; using geometry_msgs::msg::Twist; using geometry_msgs::msg::Vector3; +using path_safety_checker::ExtendedPredictedObject; +using path_safety_checker::ObjectTypesToCheck; +using path_safety_checker::PoseWithVelocityAndPolygonStamped; +using path_safety_checker::PoseWithVelocityStamped; +using path_safety_checker::PredictedPathWithPolygon; +using path_safety_checker::SafetyCheckParams; +using path_safety_checker::TargetObjectsOnLane; using route_handler::RouteHandler; using tier4_autoware_utils::LinearRing2d; using tier4_autoware_utils::LineString2d; @@ -199,27 +203,6 @@ double calcLongitudinalDistanceFromEgoToObjects( const Pose & ego_pose, double base_link2front, double base_link2rear, const PredictedObjects & dynamic_objects); -/** - * @brief Separate index of the obstacles into two part based on whether the object is within - * lanelet. - * @return Indices of objects pair. first objects are in the lanelet, and second others are out of - * lanelet. - */ -std::pair, std::vector> separateObjectIndicesByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); - -/** - * @brief Separate the objects into two part based on whether the object is within lanelet. - * @return Objects pair. first objects are in the lanelet, and second others are out of lanelet. - */ -std::pair separateObjectsByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); - -PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v); - -PredictedObjects filterObjectsByVelocity( - const PredictedObjects & objects, double min_v, double max_v); - // drivable area generation lanelet::ConstLanelets transformToLanelets(const DrivableLanes & drivable_lanes); lanelet::ConstLanelets transformToLanelets(const std::vector & drivable_lanes); @@ -391,9 +374,6 @@ lanelet::ConstLanelets calcLaneAroundPose( const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); -std::vector getPredictedPathFromObj( - const ExtendedPredictedObject & obj, const bool & is_use_all_predicted_path); - bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold); double calcMinimumLaneChangeLength( diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 88bc583345966..2a806bcb24cfe 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -17,6 +17,7 @@ #include "behavior_path_planner/marker_utils/avoidance/debug.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/avoidance/utils.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -1895,11 +1896,11 @@ bool AvoidanceModule::isSafePath( avoidance_data_, planner_data_, parameters_, is_right_shift.value()); for (const auto & object : safety_check_target_objects) { - const auto obj_predicted_paths = - utils::getPredictedPathFromObj(object, parameters_->check_all_predicted_path); + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( + object, parameters_->check_all_predicted_path); for (const auto & obj_path : obj_predicted_paths) { CollisionCheckDebug collision{}; - if (!utils::safety_check::checkCollision( + if (!utils::path_safety_checker::checkCollision( shifted_path.path, ego_predicted_path, object, obj_path, p, p.expected_front_deceleration, p.expected_rear_deceleration, collision)) { return false; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp index 0db16c4c1d4ca..841fc9092e279 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp @@ -15,6 +15,7 @@ #include "behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp" #include "behavior_path_planner/utils/avoidance/utils.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include @@ -136,7 +137,8 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects( const auto p = std::dynamic_pointer_cast(avoidance_parameters_); const auto [object_within_target_lane, object_outside_target_lane] = - utils::separateObjectsByLanelets(*planner_data_->dynamic_object, data.current_lanelets); + utils::path_safety_checker::separateObjectsByLanelets( + *planner_data_->dynamic_object, data.current_lanelets); // Assume that the maximum allocation for data.other object is the sum of // objects_within_target_lane and object_outside_target_lane. The maximum allocation for 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 29cefe110d9e4..77c5e78d3db50 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 @@ -16,6 +16,7 @@ #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/lane_change/utils.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -1372,18 +1373,19 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( for (const auto & obj : collision_check_objects) { auto current_debug_data = assignDebugData(obj); current_debug_data.second.ego_predicted_path.push_back(debug_predicted_path); - const auto obj_predicted_paths = - utils::getPredictedPathFromObj(obj, lane_change_parameters_->use_all_predicted_path); + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( + obj, lane_change_parameters_->use_all_predicted_path); for (const auto & obj_path : obj_predicted_paths) { - if (!utils::safety_check::checkCollision( + if (!utils::path_safety_checker::checkCollision( path, ego_predicted_path, obj, obj_path, common_parameters, front_decel, rear_decel, current_debug_data.second)) { path_safety_status.is_safe = false; updateDebugInfo(current_debug_data, path_safety_status.is_safe); const auto & obj_pose = obj.initial_pose.pose; const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape); - path_safety_status.is_object_coming_from_rear |= !utils::safety_check::isTargetObjectFront( - path, current_pose, common_parameters.vehicle_info, obj_polygon); + path_safety_status.is_object_coming_from_rear |= + !utils::path_safety_checker::isTargetObjectFront( + path, current_pose, common_parameters.vehicle_info, obj_polygon); } } updateDebugInfo(current_debug_data, path_safety_status.is_safe); 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 4e27f57b39e7b..860cdac31640d 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 @@ -15,9 +15,9 @@ #include "behavior_path_planner/scene_module/start_planner/start_planner_module.hpp" #include "behavior_path_planner/utils/create_vehicle_footprint.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" -#include "behavior_path_planner/utils/utils.hpp" #include #include @@ -631,9 +631,10 @@ std::vector StartPlannerModule::searchPullOutStartPoses() // filter pull out lanes stop objects const auto [pull_out_lane_objects, others] = - utils::separateObjectsByLanelets(*planner_data_->dynamic_object, status_.pull_out_lanes); - const auto pull_out_lane_stop_objects = - utils::filterObjectsByVelocity(pull_out_lane_objects, parameters_->th_moving_object_velocity); + utils::path_safety_checker::separateObjectsByLanelets( + *planner_data_->dynamic_object, status_.pull_out_lanes); + const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + pull_out_lane_objects, parameters_->th_moving_object_velocity); // lateral shift to current_pose const double distance_from_center_line = arc_position_pose.distance; diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index 9daee39433d97..1e57dcc319dc5 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -15,6 +15,7 @@ #include "behavior_path_planner/utils/goal_planner/goal_searcher.hpp" #include "behavior_path_planner/utils/goal_planner/util.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "lanelet2_extension/utility/utilities.hpp" @@ -69,7 +70,8 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) parameters_.goal_search_interval); const auto [shoulder_lane_objects, others] = - utils::separateObjectsByLanelets(*(planner_data_->dynamic_object), pull_over_lanes); + utils::path_safety_checker::separateObjectsByLanelets( + *(planner_data_->dynamic_object), pull_over_lanes); std::vector original_search_poses{}; // for search area visualizing size_t goal_id = 0; @@ -151,7 +153,8 @@ void GoalSearcher::update(GoalCandidates & goal_candidates) const const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); const auto [shoulder_lane_objects, others] = - utils::separateObjectsByLanelets(*(planner_data_->dynamic_object), pull_over_lanes); + utils::path_safety_checker::separateObjectsByLanelets( + *(planner_data_->dynamic_object), pull_over_lanes); constexpr bool filter_inside = true; const auto target_objects = goal_planner_utils::filterObjectsByLateralDistance( goal_pose, planner_data_->parameters.vehicle_width, shoulder_lane_objects, 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 new file mode 100644 index 0000000000000..33afe0fe6642f --- /dev/null +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp @@ -0,0 +1,353 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" + +#include "behavior_path_planner/utils/utils.hpp" + +namespace behavior_path_planner::utils::path_safety_checker +{ + +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) +{ + // 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; + + PredictedObjects filtered_objects; + + filtered_objects = filterObjectsByVelocity(*objects, ignore_object_velocity_threshold); + + filterObjectsByClass(filtered_objects, target_object_types); + + const auto path = route_handler->getCenterLinePath( + 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); + + return filtered_objects; +} + +PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v) +{ + return filterObjectsByVelocity(objects, -lim_v, lim_v); +} + +PredictedObjects filterObjectsByVelocity( + const PredictedObjects & objects, double min_v, double max_v) +{ + PredictedObjects filtered; + filtered.header = objects.header; + for (const auto & obj : objects.objects) { + const auto v_norm = std::hypot( + obj.kinematics.initial_twist_with_covariance.twist.linear.x, + obj.kinematics.initial_twist_with_covariance.twist.linear.y); + if (min_v < v_norm && v_norm < max_v) { + filtered.objects.push_back(obj); + } + } + 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) +{ + // Create a new container to hold the filtered objects + PredictedObjects filtered; + filtered.header = objects.header; + + // Reserve space in the vector to avoid reallocations + filtered.objects.reserve(objects.objects.size()); + + for (const auto & obj : objects.objects) { + const double dist_ego_to_obj = motion_utils::calcSignedArcLength( + path_points, current_pose, obj.kinematics.initial_pose_with_covariance.pose.position); + + if (-backward_distance < dist_ego_to_obj && dist_ego_to_obj < forward_distance) { + filtered.objects.push_back(obj); + } + } + + // Replace the original objects with the filtered list + objects.objects = std::move(filtered.objects); + return; +} + +void filterObjectsByClass( + PredictedObjects & objects, const ObjectTypesToCheck & target_object_types) +{ + using autoware_auto_perception_msgs::msg::ObjectClassification; + + PredictedObjects filtered_objects; + + for (auto & object : objects.objects) { + const auto t = utils::getHighestProbLabel(object.classification); + const auto is_object_type = + ((t == ObjectClassification::CAR && target_object_types.check_car) || + (t == ObjectClassification::TRUCK && target_object_types.check_truck) || + (t == ObjectClassification::BUS && target_object_types.check_bus) || + (t == ObjectClassification::TRAILER && target_object_types.check_trailer) || + (t == ObjectClassification::UNKNOWN && target_object_types.check_unknown) || + (t == ObjectClassification::BICYCLE && target_object_types.check_bicycle) || + (t == ObjectClassification::MOTORCYCLE && target_object_types.check_motorcycle) || + (t == ObjectClassification::PEDESTRIAN && target_object_types.check_pedestrian)); + + // If the object type matches any of the target types, add it to the filtered list + if (is_object_type) { + filtered_objects.objects.push_back(object); + } + } + + // Replace the original objects with the filtered list + objects = std::move(filtered_objects); + + return; +} + +std::pair, std::vector> separateObjectIndicesByLanelets( + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) +{ + if (target_lanelets.empty()) { + return {}; + } + + std::vector target_indices; + std::vector other_indices; + + for (size_t i = 0; i < objects.objects.size(); i++) { + // create object polygon + const auto & obj = objects.objects.at(i); + // create object polygon + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj); + bool is_filtered_object = false; + for (const auto & llt : target_lanelets) { + // create lanelet polygon + const auto polygon2d = llt.polygon2d().basicPolygon(); + if (polygon2d.empty()) { + // no lanelet polygon + continue; + } + Polygon2d lanelet_polygon; + for (const auto & lanelet_point : polygon2d) { + lanelet_polygon.outer().emplace_back(lanelet_point.x(), lanelet_point.y()); + } + lanelet_polygon.outer().push_back(lanelet_polygon.outer().front()); + // check the object does not intersect the lanelet + if (!boost::geometry::disjoint(lanelet_polygon, obj_polygon)) { + target_indices.push_back(i); + is_filtered_object = true; + break; + } + } + + if (!is_filtered_object) { + other_indices.push_back(i); + } + } + + return std::make_pair(target_indices, other_indices); +} + +std::pair separateObjectsByLanelets( + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) +{ + PredictedObjects target_objects; + PredictedObjects other_objects; + + const auto [target_indices, other_indices] = + separateObjectIndicesByLanelets(objects, target_lanelets); + + target_objects.objects.reserve(target_indices.size()); + other_objects.objects.reserve(other_indices.size()); + + for (const size_t i : target_indices) { + target_objects.objects.push_back(objects.objects.at(i)); + } + + for (const size_t i : other_indices) { + other_objects.objects.push_back(objects.objects.at(i)); + } + + return std::make_pair(target_objects, other_objects); +} + +std::vector getPredictedPathFromObj( + const ExtendedPredictedObject & obj, const bool & is_use_all_predicted_path) +{ + if (!is_use_all_predicted_path) { + const auto max_confidence_path = std::max_element( + obj.predicted_paths.begin(), obj.predicted_paths.end(), + [](const auto & path1, const auto & path2) { return path1.confidence < path2.confidence; }); + if (max_confidence_path != obj.predicted_paths.end()) { + return {*max_confidence_path}; + } + } + + return obj.predicted_paths; +} + +// TODO(Sugahara): should consider delay before departure +std::vector createPredictedPath( + const std::shared_ptr & ego_predicted_path_params, + const std::vector & path_points, + const geometry_msgs::msg::Pose & vehicle_pose, const double current_velocity, size_t ego_seg_idx) +{ + if (path_points.empty()) { + return {}; + } + + const double min_slow_down_speed = ego_predicted_path_params->min_slow_speed; + const double acceleration = ego_predicted_path_params->acceleration; + const double time_horizon = ego_predicted_path_params->time_horizon; + const double time_resolution = ego_predicted_path_params->time_resolution; + + std::vector predicted_path; + const auto vehicle_pose_frenet = + convertToFrenetPoint(path_points, vehicle_pose.position, ego_seg_idx); + + for (double t = 0.0; t < time_horizon + 1e-3; t += time_resolution) { + const double velocity = std::max(current_velocity + acceleration * t, min_slow_down_speed); + const double length = current_velocity * t + 0.5 * acceleration * t * t; + const auto pose = + motion_utils::calcInterpolatedPose(path_points, vehicle_pose_frenet.length + length); + predicted_path.emplace_back(t, pose, velocity); + } + + return predicted_path; +} + +bool isCentroidWithinLanelets( + const PredictedObject & object, const lanelet::ConstLanelets & target_lanelets) +{ + if (target_lanelets.empty()) { + return false; + } + + const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position; + lanelet::BasicPoint2d object_centroid(object_pos.x, object_pos.y); + + for (const auto & llt : target_lanelets) { + if (boost::geometry::within(object_centroid, llt.polygon2d().basicPolygon())) { + return true; + } + } + + return false; +} + +ExtendedPredictedObject transform( + const PredictedObject & object, const double safety_check_time_horizon, + const double safety_check_time_resolution) +{ + ExtendedPredictedObject extended_object; + extended_object.uuid = object.object_id; + extended_object.initial_pose = object.kinematics.initial_pose_with_covariance; + extended_object.initial_twist = object.kinematics.initial_twist_with_covariance; + extended_object.initial_acceleration = object.kinematics.initial_acceleration_with_covariance; + extended_object.shape = object.shape; + + const auto obj_velocity = extended_object.initial_twist.twist.linear.x; + + extended_object.predicted_paths.resize(object.kinematics.predicted_paths.size()); + for (size_t i = 0; i < object.kinematics.predicted_paths.size(); ++i) { + const auto & path = object.kinematics.predicted_paths[i]; + extended_object.predicted_paths[i].confidence = path.confidence; + + // Create path based on time horizon and resolution + for (double t = 0.0; t < safety_check_time_horizon + 1e-3; t += safety_check_time_resolution) { + const auto obj_pose = object_recognition_utils::calcInterpolatedPose(path, t); + if (obj_pose) { + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(*obj_pose, object.shape); + extended_object.predicted_paths[i].path.emplace_back( + t, *obj_pose, obj_velocity, obj_polygon); + } + } + } + + return extended_object; +} + +TargetObjectsOnLane createTargetObjectsOnLane( + const lanelet::ConstLanelets & current_lanes, const std::shared_ptr & route_handler, + const PredictedObjects & filtered_objects, const std::shared_ptr & params) +{ + const auto & object_lane_configuration = params->object_lane_configuration; + const bool include_opposite = params->include_opposite_lane; + const bool invert_opposite = params->invert_opposite_lane; + const double safety_check_time_horizon = params->safety_check_time_horizon; + const double safety_check_time_resolution = params->safety_check_time_resolution; + + lanelet::ConstLanelets all_left_lanelets; + lanelet::ConstLanelets all_right_lanelets; + + // Define lambda functions to update left and right lanelets + const auto update_left_lanelets = [&](const lanelet::ConstLanelet & target_lane) { + const auto left_lanelets = route_handler->getAllLeftSharedLinestringLanelets( + target_lane, include_opposite, invert_opposite); + all_left_lanelets.insert(all_left_lanelets.end(), left_lanelets.begin(), left_lanelets.end()); + }; + + const auto update_right_lanelets = [&](const lanelet::ConstLanelet & target_lane) { + const auto right_lanelets = route_handler->getAllRightSharedLinestringLanelets( + target_lane, include_opposite, invert_opposite); + all_right_lanelets.insert( + all_right_lanelets.end(), right_lanelets.begin(), right_lanelets.end()); + }; + + // Update left and right lanelets for each current lane + for (const auto & current_lane : current_lanes) { + update_left_lanelets(current_lane); + update_right_lanelets(current_lane); + } + + TargetObjectsOnLane target_objects_on_lane{}; + const auto append_objects_on_lane = [&](auto & lane_objects, const auto & check_lanes) { + std::for_each( + filtered_objects.objects.begin(), filtered_objects.objects.end(), [&](const auto & object) { + if (isCentroidWithinLanelets(object, check_lanes)) { + lane_objects.push_back( + transform(object, safety_check_time_horizon, safety_check_time_resolution)); + } + }); + }; + + // TODO(Sugahara): Consider shoulder and other lane objects + if (object_lane_configuration.check_current_lane) { + append_objects_on_lane(target_objects_on_lane.on_current_lane, current_lanes); + } + if (object_lane_configuration.check_left_lane) { + append_objects_on_lane(target_objects_on_lane.on_left_lane, all_left_lanelets); + } + if (object_lane_configuration.check_right_lane) { + append_objects_on_lane(target_objects_on_lane.on_right_lane, all_right_lanelets); + } + + return target_objects_on_lane; +} + +} // namespace behavior_path_planner::utils::path_safety_checker diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index 23c2951d30a38..ee23ecc005142 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -19,7 +19,7 @@ #include "motion_utils/trajectory/trajectory.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" -namespace behavior_path_planner::utils::safety_check +namespace behavior_path_planner::utils::path_safety_checker { void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point) { @@ -303,4 +303,4 @@ bool checkCollision( return true; } -} // namespace behavior_path_planner::utils::safety_check +} // namespace behavior_path_planner::utils::path_safety_checker diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp index 5eac8291f0e1a..721ffd3064840 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp @@ -14,6 +14,7 @@ #include "behavior_path_planner/utils/start_planner/geometric_pull_out.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -69,9 +70,10 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p // collision check with stop objects in pull out lanes const auto arc_path = planner_.getArcPath(); const auto [pull_out_lane_objects, others] = - utils::separateObjectsByLanelets(*(planner_data_->dynamic_object), pull_out_lanes); - const auto pull_out_lane_stop_objects = - utils::filterObjectsByVelocity(pull_out_lane_objects, parameters_.th_moving_object_velocity); + utils::path_safety_checker::separateObjectsByLanelets( + *(planner_data_->dynamic_object), pull_out_lanes); + const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + pull_out_lane_objects, parameters_.th_moving_object_velocity); if (utils::checkCollisionBetweenPathFootprintsAndObjects( vehicle_footprint_, arc_path, pull_out_lane_stop_objects, diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp index cbe06fa66c306..08dcc0254deb1 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp @@ -14,6 +14,7 @@ #include "behavior_path_planner/utils/start_planner/shift_pull_out.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -64,9 +65,9 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) // extract stop objects in pull out lane for collision check const auto [pull_out_lane_objects, others] = - utils::separateObjectsByLanelets(*dynamic_objects, pull_out_lanes); - const auto pull_out_lane_stop_objects = - utils::filterObjectsByVelocity(pull_out_lane_objects, parameters_.th_moving_object_velocity); + utils::path_safety_checker::separateObjectsByLanelets(*dynamic_objects, pull_out_lanes); + const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + pull_out_lane_objects, parameters_.th_moving_object_velocity); // get safe path for (auto & pull_out_path : pull_out_paths) { diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 8cc94bd6b46fa..b1f5afc119b39 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -711,73 +711,6 @@ double calcLongitudinalDistanceFromEgoToObjects( return min_distance; } -std::pair, std::vector> separateObjectIndicesByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) -{ - if (target_lanelets.empty()) { - return {}; - } - - std::vector target_indices; - std::vector other_indices; - - for (size_t i = 0; i < objects.objects.size(); i++) { - // create object polygon - const auto & obj = objects.objects.at(i); - // create object polygon - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj); - bool is_filtered_object = false; - for (const auto & llt : target_lanelets) { - // create lanelet polygon - const auto polygon2d = llt.polygon2d().basicPolygon(); - if (polygon2d.empty()) { - // no lanelet polygon - continue; - } - Polygon2d lanelet_polygon; - for (const auto & lanelet_point : polygon2d) { - lanelet_polygon.outer().emplace_back(lanelet_point.x(), lanelet_point.y()); - } - lanelet_polygon.outer().push_back(lanelet_polygon.outer().front()); - // check the object does not intersect the lanelet - if (!boost::geometry::disjoint(lanelet_polygon, obj_polygon)) { - target_indices.push_back(i); - is_filtered_object = true; - break; - } - } - - if (!is_filtered_object) { - other_indices.push_back(i); - } - } - - return std::make_pair(target_indices, other_indices); -} - -std::pair separateObjectsByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) -{ - PredictedObjects target_objects; - PredictedObjects other_objects; - - const auto [target_indices, other_indices] = - separateObjectIndicesByLanelets(objects, target_lanelets); - - target_objects.objects.reserve(target_indices.size()); - other_objects.objects.reserve(other_indices.size()); - - for (const size_t i : target_indices) { - target_objects.objects.push_back(objects.objects.at(i)); - } - - for (const size_t i : other_indices) { - other_objects.objects.push_back(objects.objects.at(i)); - } - - return std::make_pair(target_objects, other_objects); -} - std::vector calcObjectsDistanceToPath( const PredictedObjects & objects, const PathWithLaneId & ego_path) { @@ -2661,25 +2594,6 @@ std::vector expandLanelets( return expanded_drivable_lanes; } -PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v) -{ - return filterObjectsByVelocity(objects, -lim_v, lim_v); -} - -PredictedObjects filterObjectsByVelocity( - const PredictedObjects & objects, double min_v, double max_v) -{ - PredictedObjects filtered; - filtered.header = objects.header; - for (const auto & obj : objects.objects) { - const auto v = std::abs(obj.kinematics.initial_twist_with_covariance.twist.linear.x); - if (min_v < v && v < max_v) { - filtered.objects.push_back(obj); - } - } - return filtered; -} - PathWithLaneId getCenterLinePathFromRootLanelet( const lanelet::ConstLanelet & root_lanelet, const std::shared_ptr & planner_data) @@ -3051,21 +2965,6 @@ lanelet::ConstLanelets calcLaneAroundPose( return current_lanes; } -std::vector getPredictedPathFromObj( - const ExtendedPredictedObject & obj, const bool & is_use_all_predicted_path) -{ - if (!is_use_all_predicted_path) { - const auto max_confidence_path = std::max_element( - obj.predicted_paths.begin(), obj.predicted_paths.end(), - [](const auto & path1, const auto & path2) { return path1.confidence < path2.confidence; }); - if (max_confidence_path != obj.predicted_paths.end()) { - return {*max_confidence_path}; - } - } - - return obj.predicted_paths; -} - bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold) { // We need at least three points to compute relative angle diff --git a/planning/behavior_path_planner/test/test_safety_check.cpp b/planning/behavior_path_planner/test/test_safety_check.cpp index 4058956ceff3f..e24f3274179fe 100644 --- a/planning/behavior_path_planner/test/test_safety_check.cpp +++ b/planning/behavior_path_planner/test/test_safety_check.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "behavior_path_planner/marker_utils/utils.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" @@ -35,7 +36,7 @@ using tier4_autoware_utils::Polygon2d; TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) { - using behavior_path_planner::utils::safety_check::createExtendedPolygon; + using behavior_path_planner::utils::path_safety_checker::createExtendedPolygon; vehicle_info_util::VehicleInfo vehicle_info; vehicle_info.max_longitudinal_offset_m = 4.0; @@ -128,7 +129,7 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon) { - using behavior_path_planner::utils::safety_check::createExtendedPolygon; + using behavior_path_planner::utils::path_safety_checker::createExtendedPolygon; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::createQuaternionFromYaw; @@ -176,7 +177,7 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon) TEST(BehaviorPathPlanningSafetyUtilsTest, calcRssDistance) { - using behavior_path_planner::utils::safety_check::calcRssDistance; + using behavior_path_planner::utils::path_safety_checker::calcRssDistance; { const double front_vel = 5.0;