From 06f4052349c3178249f85407d4f4467bc4570442 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 29 Aug 2023 10:05:42 +0900 Subject: [PATCH] perf(safety_check): use light weight object position check logic (#4762) perf(safety_check): use light weight object position check Signed-off-by: satoshi-ota --- .../path_safety_checker/safety_check.hpp | 3 +++ .../avoidance/avoidance_module.cpp | 4 ++-- .../path_safety_checker/safety_check.cpp | 24 ++++++++++++++++--- 3 files changed, 26 insertions(+), 5 deletions(-) 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 79947d826ed10..7b5b1f562f1b8 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 @@ -52,6 +52,9 @@ using vehicle_info_util::VehicleInfo; namespace bg = boost::geometry; +bool isTargetObjectFront( + const geometry_msgs::msg::Pose & ego_pose, const Polygon2d & obj_polygon, + const vehicle_info_util::VehicleInfo & vehicle_info); bool isTargetObjectFront( const PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, const vehicle_info_util::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon); 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 91fe2d0fa6da7..f68e54266fe85 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 @@ -1880,8 +1880,8 @@ bool AvoidanceModule::isSafePath( const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape); - const auto is_object_front = utils::path_safety_checker::isTargetObjectFront( - shifted_path.path, getEgoPose(), p.vehicle_info, obj_polygon); + const auto is_object_front = + utils::path_safety_checker::isTargetObjectFront(getEgoPose(), obj_polygon, p.vehicle_info); const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( object, parameters_->check_all_predicted_path); 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 15a23c8f27dff..2939ca29c0a40 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 @@ -30,6 +30,25 @@ void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & bg::append(polygon.outer(), point); } +bool isTargetObjectFront( + const geometry_msgs::msg::Pose & ego_pose, const Polygon2d & obj_polygon, + const vehicle_info_util::VehicleInfo & vehicle_info) +{ + const double base_to_front = vehicle_info.max_longitudinal_offset_m; + const auto ego_offset_pose = + tier4_autoware_utils::calcOffsetPose(ego_pose, base_to_front, 0.0, 0.0); + + // check all edges in the polygon + for (const auto & obj_edge : obj_polygon.outer()) { + const auto obj_point = tier4_autoware_utils::createPoint(obj_edge.x(), obj_edge.y(), 0.0); + if (tier4_autoware_utils::calcLongitudinalDeviation(ego_offset_pose, obj_point) > 0.0) { + return true; + } + } + + return false; +} + bool isTargetObjectFront( const PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, const vehicle_info_util::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon) @@ -227,7 +246,7 @@ boost::optional getInterpolatedPoseWithVeloci } bool checkCollision( - const PathWithLaneId & planned_path, + [[maybe_unused]] const PathWithLaneId & planned_path, const std::vector & predicted_ego_path, const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, @@ -272,8 +291,7 @@ bool checkCollision( } // compute which one is at the front of the other - const bool is_object_front = - isTargetObjectFront(planned_path, ego_pose, ego_vehicle_info, obj_polygon); + const bool is_object_front = isTargetObjectFront(ego_pose, obj_polygon, ego_vehicle_info); const auto & [front_object_velocity, rear_object_velocity] = is_object_front ? std::make_pair(object_velocity, ego_velocity) : std::make_pair(ego_velocity, object_velocity);