From 68abc992ea08539e1f73d06630c4cfe84c42ea00 Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Fri, 27 Sep 2024 14:59:11 +0900 Subject: [PATCH] feat(lane_change): modify lane change target boundary check to consider velocity (#8961) * check if candidate path footprint exceeds target lane boundary when lc velocity is above minimum Signed-off-by: mohammad alqudah * move functions to relevant module Signed-off-by: mohammad alqudah * suppress unused function cppcheck Signed-off-by: mohammad alqudah * minor change Signed-off-by: mohammad alqudah --------- Signed-off-by: mohammad alqudah --- .../src/interface.cpp | 3 + .../src/manager.cpp | 4 +- .../src/manager.hpp | 4 +- .../src/scene.cpp | 46 ++++++++++++++- .../utils/utils.hpp | 15 +---- .../src/scene.cpp | 12 ++++ .../src/utils/utils.cpp | 57 +------------------ 7 files changed, 66 insertions(+), 75 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp index fffb86767b0a8..d47f76e399b4c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp @@ -44,11 +44,14 @@ AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( { } +// cppcheck-suppress unusedFunction bool AvoidanceByLaneChangeInterface::isExecutionRequested() const { return module_type_->isLaneChangeRequired() && module_type_->specialRequiredCheck() && module_type_->isValidPath(); } + +// cppcheck-suppress unusedFunction void AvoidanceByLaneChangeInterface::processOnEntry() { waitApproval(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp index ef02bfaa7b0dd..ea5b853051e9a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -186,8 +186,8 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) avoidance_parameters_ = std::make_shared(p); } -std::unique_ptr -AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() +// cppcheck-suppress unusedFunction +SMIPtr AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() { return std::make_unique( name_, *node_, parameters_, avoidance_parameters_, rtc_interface_ptr_map_, diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp index 95609a430ac80..44f842a67c236 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp @@ -32,6 +32,8 @@ using autoware::behavior_path_planner::LaneChangeModuleManager; using autoware::behavior_path_planner::LaneChangeModuleType; using autoware::behavior_path_planner::SceneModuleInterface; +using SMIPtr = std::unique_ptr; + class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager { public: @@ -44,7 +46,7 @@ class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager void init(rclcpp::Node * node) override; - std::unique_ptr createNewSceneModuleInstance() override; + SMIPtr createNewSceneModuleInstance() override; private: std::shared_ptr avoidance_parameters_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp index 25a155c324a55..3f56320af171e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -34,13 +34,55 @@ #include #include +namespace +{ +geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose) +{ + geometry_msgs::msg::Point32 p; + p.x = static_cast(pose.position.x); + p.y = static_cast(pose.position.y); + p.z = static_cast(pose.position.z); + return p; +}; + +geometry_msgs::msg::Polygon create_execution_area( + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const geometry_msgs::msg::Pose & pose, double additional_lon_offset, double additional_lat_offset) +{ + const double & base_to_front = vehicle_info.max_longitudinal_offset_m; + const double & width = vehicle_info.vehicle_width_m; + const double & base_to_rear = vehicle_info.rear_overhang_m; + + // if stationary object, extend forward and backward by the half of lon length + const double forward_lon_offset = base_to_front + additional_lon_offset; + const double backward_lon_offset = -base_to_rear; + const double lat_offset = width / 2.0 + additional_lat_offset; + + const auto p1 = + autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, lat_offset, 0.0); + const auto p2 = + autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, -lat_offset, 0.0); + const auto p3 = + autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, -lat_offset, 0.0); + const auto p4 = + autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, lat_offset, 0.0); + geometry_msgs::msg::Polygon polygon; + + polygon.points.push_back(create_point32(p1)); + polygon.points.push_back(create_point32(p2)); + polygon.points.push_back(create_point32(p3)); + polygon.points.push_back(create_point32(p4)); + + return polygon; +} +} // namespace + namespace autoware::behavior_path_planner { using autoware::behavior_path_planner::Direction; using autoware::behavior_path_planner::LaneChangeModuleType; using autoware::behavior_path_planner::ObjectInfo; using autoware::behavior_path_planner::Point2d; -using autoware::behavior_path_planner::utils::lane_change::debug::createExecutionArea; AvoidanceByLaneChange::AvoidanceByLaneChange( const std::shared_ptr & parameters, @@ -84,7 +126,7 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const const auto minimum_avoid_length = calcMinAvoidanceLength(nearest_object); const auto minimum_lane_change_length = calcMinimumLaneChangeLength(); - lane_change_debug_.execution_area = createExecutionArea( + lane_change_debug_.execution_area = create_execution_area( getCommonParam().vehicle_info, getEgoPose(), std::max(minimum_lane_change_length, minimum_avoid_length), calcLateralOffset()); 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 c9990747f57a7..260ef78807ff7 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 @@ -91,7 +91,7 @@ bool isPathInLanelets( const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); -bool pathFootprintExceedsTargetLaneBound( +bool path_footprint_exceeds_target_lane_bound( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info, const double margin = 0.1); @@ -257,10 +257,6 @@ bool is_same_lane_with_prev_iteration( const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); -Pose to_pose( - const autoware::universe_utils::Point2d & point, - const geometry_msgs::msg::Quaternion & orientation); - bool is_ahead_of_ego( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const PredictedObject & object); @@ -280,13 +276,4 @@ double get_distance_to_next_regulatory_element( const bool ignore_intersection = false); } // namespace autoware::behavior_path_planner::utils::lane_change -namespace autoware::behavior_path_planner::utils::lane_change::debug -{ -geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose); - -geometry_msgs::msg::Polygon createExecutionArea( - const VehicleInfo & vehicle_info, const Pose & pose, double additional_lon_offset, - double additional_lat_offset); -} // namespace autoware::behavior_path_planner::utils::lane_change::debug - #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ 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 e8a63c09580e1..6a928941e71ef 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 @@ -1609,6 +1609,18 @@ bool NormalLaneChange::check_candidate_path_safety( "Ego is not stuck and parked vehicle exists in the target lane. Skip lane change."); } + const auto lc_start_velocity = candidate_path.info.velocity.prepare; + const auto min_lc_velocity = lane_change_parameters_->minimum_lane_changing_velocity; + constexpr double margin = 0.1; + // path is unsafe if it exceeds target lane boundary with a high velocity + if ( + lane_change_parameters_->enable_target_lane_bound_check && + lc_start_velocity > min_lc_velocity + margin && + utils::lane_change::path_footprint_exceeds_target_lane_bound( + common_data_ptr_, candidate_path.shifted_path.path, planner_data_->parameters.vehicle_info)) { + throw std::logic_error("Path footprint exceeds target lane boundary. Skip lane change."); + } + constexpr size_t decel_sampling_num = 1; const auto safety_check_with_normal_rss = isLaneChangePathSafe( candidate_path, target_objects, common_data_ptr_->lc_param_ptr->rss_params, decel_sampling_num, 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 c982c0aad988f..146e704f746b3 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 @@ -200,8 +200,7 @@ bool isPathInLanelets( return true; } -// cppcheck-suppress unusedFunction -bool pathFootprintExceedsTargetLaneBound( +bool path_footprint_exceeds_target_lane_bound( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info, const double margin) { @@ -1145,16 +1144,6 @@ bool is_same_lane_with_prev_iteration( (prev_target_lanes.back().id() == prev_target_lanes.back().id()); } -Pose to_pose( - const autoware::universe_utils::Point2d & point, - const geometry_msgs::msg::Quaternion & orientation) -{ - Pose pose; - pose.position = autoware::universe_utils::createPoint(point.x(), point.y(), 0.0); - pose.orientation = orientation; - return pose; -} - bool is_ahead_of_ego( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const PredictedObject & object) @@ -1279,47 +1268,3 @@ double get_distance_to_next_regulatory_element( return distance; } } // namespace autoware::behavior_path_planner::utils::lane_change - -namespace autoware::behavior_path_planner::utils::lane_change::debug -{ -geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose) -{ - geometry_msgs::msg::Point32 p; - p.x = static_cast(pose.position.x); - p.y = static_cast(pose.position.y); - p.z = static_cast(pose.position.z); - return p; -}; - -geometry_msgs::msg::Polygon createExecutionArea( - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const Pose & pose, - double additional_lon_offset, double additional_lat_offset) -{ - const double & base_to_front = vehicle_info.max_longitudinal_offset_m; - const double & width = vehicle_info.vehicle_width_m; - const double & base_to_rear = vehicle_info.rear_overhang_m; - - // if stationary object, extend forward and backward by the half of lon length - const double forward_lon_offset = base_to_front + additional_lon_offset; - const double backward_lon_offset = -base_to_rear; - const double lat_offset = width / 2.0 + additional_lat_offset; - - const auto p1 = - autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, lat_offset, 0.0); - const auto p2 = - autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, -lat_offset, 0.0); - const auto p3 = - autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, -lat_offset, 0.0); - const auto p4 = - autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, lat_offset, 0.0); - geometry_msgs::msg::Polygon polygon; - - polygon.points.push_back(create_point32(p1)); - polygon.points.push_back(create_point32(p2)); - polygon.points.push_back(create_point32(p3)); - polygon.points.push_back(create_point32(p4)); - - return polygon; -} - -} // namespace autoware::behavior_path_planner::utils::lane_change::debug