From 290c264a19ea3d83f16debe153bf13cda5f035c6 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Thu, 22 Feb 2024 13:13:01 +0900 Subject: [PATCH] fix(avoidance_by_lane_change): loosen execution condition Signed-off-by: Muhammad Zulfaqar Azmi --- .../scene.hpp | 4 + .../src/interface.cpp | 4 - .../src/scene.cpp | 148 +++++++----------- .../utils/debug_structs.hpp | 3 + .../utils/markers.hpp | 3 + .../utils/utils.hpp | 10 ++ .../src/scene.cpp | 2 - .../src/utils/markers.cpp | 5 + .../src/utils/utils.cpp | 39 +++++ 9 files changed, 119 insertions(+), 99 deletions(-) diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp index 7a82ba60a1eaa..c663c31537fd1 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp @@ -54,6 +54,10 @@ class AvoidanceByLaneChange : public NormalLaneChange const AvoidancePlanningData & data, const PredictedObject & object) const; void fillAvoidanceTargetObjects(AvoidancePlanningData & data, AvoidanceDebugData & debug) const; + + double calcMinAvoidanceLength(const ObjectData & nearest_object) const; + double calcMinimumLaneChangeLength() const; + double calcLateralOffset() const; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp index 985fbcd15072c..3a7c22c84b3e8 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp @@ -16,13 +16,9 @@ #include "behavior_path_planner_common/interface/scene_module_interface.hpp" #include "behavior_path_planner_common/interface/scene_module_visitor.hpp" -#include "behavior_path_planner_common/marker_utils/utils.hpp" -#include #include #include -#include -#include namespace behavior_path_planner { diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp index 6147ee665bab6..69bc82ef6483f 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -21,6 +21,8 @@ #include "behavior_path_planner_common/utils/utils.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" +#include +#include #include #include #include @@ -32,11 +34,13 @@ #include #include -#include +#include #include namespace behavior_path_planner { +using behavior_path_planner::utils::lane_change::debug::createExecutionArea; + AvoidanceByLaneChange::AvoidanceByLaneChange( const std::shared_ptr & parameters, std::shared_ptr avoidance_parameters) @@ -76,101 +80,15 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const return false; } - const auto current_lanes = getCurrentLanes(); - if (current_lanes.empty()) { - RCLCPP_DEBUG(logger_, "no empty lanes"); - return false; - } - const auto & nearest_object = data.target_objects.front(); + const auto minimum_avoid_length = calcMinAvoidanceLength(nearest_object); + const auto minimum_lane_change_length = calcMinimumLaneChangeLength(); - // get minimum lane change distance - const auto shift_intervals = - getRouteHandler()->getLateralIntervalsToPreferredLane(current_lanes.back(), direction_); - const double minimum_lane_change_length = utils::lane_change::calcMinimumLaneChangeLength( - *lane_change_parameters_, shift_intervals, - lane_change_parameters_->backward_length_buffer_for_end_of_lane); - - const auto ego_width = getCommonParam().vehicle_width; - const auto nearest_object_type = utils::getHighestProbLabel(nearest_object.object.classification); - const auto nearest_object_parameter = - avoidance_parameters_->object_parameters.at(nearest_object_type); - const auto avoid_margin = - nearest_object_parameter.safety_buffer_lateral * nearest_object.distance_factor + - nearest_object_parameter.avoid_margin_lateral + 0.5 * ego_width; - - avoidance_helper_->setData(planner_data_); - const auto shift_length = avoidance_helper_->getShiftLength( - nearest_object, utils::avoidance::isOnRight(nearest_object), avoid_margin); - - const auto minimum_avoid_distance = avoidance_helper_->getMinAvoidanceDistance(shift_length); - - const auto max_offset = std::invoke([&]() -> double { - auto max_offset{0.0}; - for (const auto & [type, p] : avoidance_parameters_->object_parameters) { - const auto offset = - 2.0 * p.envelope_buffer_margin + p.safety_buffer_lateral + p.avoid_margin_lateral; - max_offset = std::max(max_offset, offset); - } - return max_offset; - }); - - auto create_point32 = [](const geometry_msgs::msg::Pose & pose) -> geometry_msgs::msg::Point32 { - 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; - }; - - const auto create_vehicle_polygon = - [&](const vehicle_info_util::VehicleInfo & vehicle_info, const double 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 + std::max(minimum_lane_change_length, minimum_avoid_distance); - const double backward_lon_offset = -base_to_rear; - const double lat_offset = width / 2.0 + offset; - - const auto & base_link_pose = getEgoPose(); - - const auto p1 = - tier4_autoware_utils::calcOffsetPose(base_link_pose, forward_lon_offset, lat_offset, 0.0); - const auto p2 = - tier4_autoware_utils::calcOffsetPose(base_link_pose, forward_lon_offset, -lat_offset, 0.0); - const auto p3 = - tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, -lat_offset, 0.0); - const auto p4 = - tier4_autoware_utils::calcOffsetPose(base_link_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; - }; - - debug_execution_.effective_area = - create_vehicle_polygon(getCommonParam().vehicle_info, max_offset); - - RCLCPP_DEBUG(logger_, "ego pose.x %.3f pose.y %.3f", getEgoPosition().x, getEgoPosition().y); - RCLCPP_DEBUG( - logger_, "effective_area p1.x %.3f, p1.y %.3f", - debug_execution_.effective_area.points.front().x, - debug_execution_.effective_area.points.front().y); + lane_change_debug_.execution_area = createExecutionArea( + getCommonParam().vehicle_info, getEgoPose(), + std::max(minimum_lane_change_length, minimum_avoid_length), calcLateralOffset()); - RCLCPP_DEBUG( - logger_, - "nearest_object.longitudinal %.3f, minimum_lane_change_length %.3f, minimum_avoid_distance " - "%.3f", - nearest_object.longitudinal, minimum_lane_change_length, minimum_avoid_distance); - - return nearest_object.longitudinal > std::max(minimum_lane_change_length, minimum_avoid_distance); + return nearest_object.longitudinal > std::max(minimum_lane_change_length, minimum_avoid_length); } bool AvoidanceByLaneChange::specialExpiredCheck() const @@ -339,4 +257,48 @@ ObjectData AvoidanceByLaneChange::createObjectData( return object_data; } + +double AvoidanceByLaneChange::calcMinAvoidanceLength(const ObjectData & nearest_object) const +{ + const auto ego_width = getCommonParam().vehicle_width; + const auto nearest_object_type = utils::getHighestProbLabel(nearest_object.object.classification); + const auto nearest_object_parameter = + avoidance_parameters_->object_parameters.at(nearest_object_type); + const auto avoid_margin = + nearest_object_parameter.safety_buffer_lateral * nearest_object.distance_factor + + nearest_object_parameter.avoid_margin_lateral + 0.5 * ego_width; + + avoidance_helper_->setData(planner_data_); + const auto shift_length = avoidance_helper_->getShiftLength( + nearest_object, utils::avoidance::isOnRight(nearest_object), avoid_margin); + + return avoidance_helper_->getMinAvoidanceDistance(shift_length); +} + +double AvoidanceByLaneChange::calcMinimumLaneChangeLength() const +{ + const auto current_lanes = getCurrentLanes(); + if (current_lanes.empty()) { + RCLCPP_DEBUG(logger_, "no empty lanes"); + return std::numeric_limits::infinity(); + } + + // get minimum lane change distance + const auto shift_intervals = + getRouteHandler()->getLateralIntervalsToPreferredLane(current_lanes.back(), direction_); + return utils::lane_change::calcMinimumLaneChangeLength( + *lane_change_parameters_, shift_intervals, + lane_change_parameters_->backward_length_buffer_for_end_of_lane); +} + +double AvoidanceByLaneChange::calcLateralOffset() const +{ + auto additional_lat_offset{0.0}; + for (const auto & [type, p] : avoidance_parameters_->object_parameters) { + const auto offset = + 2.0 * p.envelope_buffer_margin + p.safety_buffer_lateral + p.avoid_margin_lateral; + additional_lat_offset = std::max(additional_lat_offset, offset); + } + return additional_lat_offset; +} } // namespace behavior_path_planner diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp index 8ac1ba3909a50..d8ba7cf19cfee 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp @@ -19,6 +19,8 @@ #include +#include + #include namespace behavior_path_planner::data::lane_change @@ -31,6 +33,7 @@ struct Debug CollisionCheckDebugMap collision_check_objects; CollisionCheckDebugMap collision_check_objects_after_approval; LaneChangeTargetObjects filtered_objects; + geometry_msgs::msg::Polygon execution_area; double collision_check_object_debug_lifetime{0.0}; void reset() diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp index 427a26e9b4295..8863cabdba008 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp @@ -19,6 +19,7 @@ #include "behavior_path_lane_change_module/utils/path.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include #include #include @@ -40,6 +41,8 @@ MarkerArray showFilteredObjects( const ExtendedPredictedObjects & current_lane_objects, const ExtendedPredictedObjects & target_lane_objects, const ExtendedPredictedObjects & other_lane_objects, const std::string & ns); +MarkerArray createExecutionArea(const geometry_msgs::msg::Polygon & execution_area); MarkerArray createDebugMarkerArray(const Debug & debug_data); + } // namespace marker_utils::lane_change_markers #endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp index 67506326d92aa..3ee8d43c0a66d 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp @@ -221,4 +221,14 @@ lanelet::ConstLanelets generateExpandedLanelets( */ rclcpp::Logger getLogger(const std::string & type); } // namespace behavior_path_planner::utils::lane_change + +namespace 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 vehicle_info_util::VehicleInfo & vehicle_info, const Pose & pose, + double additional_lon_offset, double additional_lat_offset); +} // namespace behavior_path_planner::utils::lane_change::debug + #endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 8395292c11770..61e7ea51576b4 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -15,7 +15,6 @@ #include "behavior_path_lane_change_module/scene.hpp" #include "behavior_path_lane_change_module/utils/utils.hpp" -#include "behavior_path_planner_common/marker_utils/utils.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" @@ -38,7 +37,6 @@ #include #include #include -#include #include #include diff --git a/planning/behavior_path_lane_change_module/src/utils/markers.cpp b/planning/behavior_path_lane_change_module/src/utils/markers.cpp index 523b770734bc5..67594823eb112 100644 --- a/planning/behavior_path_lane_change_module/src/utils/markers.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/markers.cpp @@ -162,6 +162,11 @@ MarkerArray createDebugMarkerArray(const Debug & debug_data) "ego_and_target_polygon_relation_after_approval")); } + if (!debug_data.execution_area.points.empty()) { + add(createPolygonMarkerArray( + debug_data.execution_area, "execution_area", 0, 0.16, 1.0, 0.69, 0.1)); + } + return debug_marker; } } // namespace marker_utils::lane_change_markers diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp index 88ea89eb7aed5..01e935b4d614a 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -1170,3 +1170,42 @@ rclcpp::Logger getLogger(const std::string & type) return rclcpp::get_logger("lane_change").get_child(type); } } // namespace behavior_path_planner::utils::lane_change + +namespace 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 vehicle_info_util::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 = tier4_autoware_utils::calcOffsetPose(pose, forward_lon_offset, lat_offset, 0.0); + const auto p2 = tier4_autoware_utils::calcOffsetPose(pose, forward_lon_offset, -lat_offset, 0.0); + const auto p3 = tier4_autoware_utils::calcOffsetPose(pose, backward_lon_offset, -lat_offset, 0.0); + const auto p4 = tier4_autoware_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 behavior_path_planner::utils::lane_change::debug