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 efdb302a58ac2..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 { @@ -45,7 +41,8 @@ AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( bool AvoidanceByLaneChangeInterface::isExecutionRequested() const { - return module_type_->isLaneChangeRequired() && module_type_->specialRequiredCheck(); + return module_type_->isLaneChangeRequired() && module_type_->specialRequiredCheck() && + module_type_->isValidPath(); } void AvoidanceByLaneChangeInterface::processOnEntry() { 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 32c92b4f4b88b..1768c184ce945 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 @@ -20,16 +20,21 @@ #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" +#include +#include #include #include #include #include +#include #include namespace behavior_path_planner { +using behavior_path_planner::utils::lane_change::debug::createExecutionArea; + namespace { lanelet::BasicLineString3d toLineString3d(const std::vector & bound) @@ -78,44 +83,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); - - // get minimum avoid distance - - 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 maximum_avoid_distance = avoidance_helper_->getMaxAvoidanceDistance(shift_length); - - RCLCPP_DEBUG( - logger_, - "nearest_object.longitudinal %.3f, minimum_lane_change_length %.3f, maximum_avoid_distance " - "%.3f", - nearest_object.longitudinal, minimum_lane_change_length, maximum_avoid_distance); + lane_change_debug_.execution_area = createExecutionArea( + getCommonParam().vehicle_info, getEgoPose(), + std::max(minimum_lane_change_length, minimum_avoid_length), calcLateralOffset()); - return nearest_object.longitudinal > std::max(minimum_lane_change_length, maximum_avoid_distance); + return nearest_object.longitudinal > std::max(minimum_lane_change_length, minimum_avoid_length); } bool AvoidanceByLaneChange::specialExpiredCheck() const @@ -284,4 +260,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 e16afcdbf19ec..46b47de128bd9 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 @@ -225,4 +225,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 c5a0f21d0f909..f913062a20d3b 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" @@ -33,7 +32,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 e6a162d2bdad5..fe582fc7bd063 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -1177,3 +1177,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