diff --git a/planning/behavior_path_lane_change_module/README.md b/planning/behavior_path_lane_change_module/README.md index a1fa2068b9ab5..564ec0a7e0abf 100644 --- a/planning/behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_lane_change_module/README.md @@ -409,6 +409,16 @@ The following parameters are configurable in `behavior_path_planner.param.yaml` | `safety_check.stuck.check_objects_on_other_lanes` | [-] | boolean | If true, the lane change module include objects on other lanes. | true | | `safety_check.stuck.use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | +| Name | Unit | Type | Description | Default value | +| :------------------------------------------------------- | ----- | ------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `enable_collision_check_for_prepare_phase.general_lanes` | [-] | boolean | Perform collision check starting from the prepare phase for situations not explicitly covered by other settings (e.g., intersections). If `false`, collision check only evaluated for lane changing phase. | false | +| `enable_collision_check_for_prepare_phase.intersection` | [-] | boolean | Perform collision check starting from prepare phase when ego is in intersection. If `false`, collision check only evaluated for lane changing phase. | true | +| `enable_collision_check_for_prepare_phase.turns` | [-] | boolean | Perform collision check starting from prepare phase when ego is in lanelet with turn direction tags. If `false`, collision check only evaluated for lane changing phase. | true | +| `prepare_phase_ignore_target_speed_thresh` | [m/s] | double | Ignore collision check in prepare phase of object speed that is lesser that the configured value. `enable_collision_check_at_prepare_phase` must be `true` | 0.1 | +| `check_objects_on_current_lanes` | [-] | boolean | If true, the lane change module check objects on current lanes when performing collision assessment. | false | +| `check_objects_on_other_lanes` | [-] | boolean | If true, the lane change module include objects on other lanes. when performing collision assessment | false | +| `use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | + (\*1) the value must be negative. ### Abort lane change diff --git a/planning/behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_lane_change_module/config/lane_change.param.yaml index 0cb7f3f1a7a92..228307a51268b 100644 --- a/planning/behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_lane_change_module/config/lane_change.param.yaml @@ -89,7 +89,10 @@ stop_time: 3.0 # [s] # collision check - enable_prepare_segment_collision_check: true + enable_collision_check_for_prepare_phase: + general_lanes: false + intersection: true + turns: true prepare_segment_ignore_object_velocity_thresh: 0.1 # [m/s] check_objects_on_current_lanes: true check_objects_on_other_lanes: true diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp index 50766621b2f44..9c396c74301c4 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp @@ -166,6 +166,8 @@ class NormalLaneChange : public LaneChangeBase bool isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const; + bool check_prepare_phase() const; + double calcMaximumLaneChangeLength( const lanelet::ConstLanelet & current_terminal_lanelet, const double max_acc) const; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp index dd7760d31eaa7..9b1814a396574 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp @@ -107,7 +107,9 @@ struct LaneChangeParameters double max_longitudinal_acc{1.0}; // collision check - bool enable_prepare_segment_collision_check{true}; + bool enable_collision_check_for_prepare_phase_in_general_lanes{false}; + bool enable_collision_check_for_prepare_phase_in_intersection{true}; + bool enable_collision_check_for_prepare_phase_in_turns{true}; double prepare_segment_ignore_object_velocity_thresh{0.1}; bool check_objects_on_current_lanes{true}; bool check_objects_on_other_lanes{true}; 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..a84acf4decd8d 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 @@ -23,6 +23,7 @@ #include "rclcpp/logger.hpp" #include +#include #include #include @@ -31,6 +32,7 @@ #include +#include #include #include @@ -185,7 +187,7 @@ std::optional createPolygon( ExtendedPredictedObject transform( const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters, - const LaneChangeParameters & lane_change_parameters); + const LaneChangeParameters & lane_change_parameters, const bool check_at_prepare_phase); bool isCollidedPolygonsInLanelet( const std::vector & collided_polygons, const lanelet::ConstLanelets & lanes); @@ -220,5 +222,72 @@ lanelet::ConstLanelets generateExpandedLanelets( * @return rclcpp::Logger The logger instance configured for the specified lane change type. */ rclcpp::Logger getLogger(const std::string & type); + +/** + * @brief Computes the current footprint of the ego vehicle based on its pose and size. + * + * This function calculates the 2D polygon representing the current footprint of the ego vehicle. + * The footprint is determined by the vehicle's pose and its dimensions, including the distance + * from the base to the front and rear ends of the vehicle, as well as its width. + * + * @param ego_pose The current pose of the ego vehicle. + * @param ego_info The structural information of the ego vehicle, such as its maximum longitudinal + * offset, rear overhang, and width. + * + * @return Polygon2d A polygon representing the current 2D footprint of the ego vehicle. + */ +Polygon2d getEgoCurrentFootprint( + const Pose & ego_pose, const vehicle_info_util::VehicleInfo & ego_info); + +/** + * @brief Checks if the given polygon is within an intersection area. + * + * This function evaluates whether a specified polygon is located within the bounds of an + * intersection. It identifies the intersection area by checking the attributes of the provided + * lanelet. If the lanelet has an attribute indicating it is part of an intersection, the function + * then checks if the polygon is fully contained within this area. + * + * @param route_handler a shared pointer to the route_handler + * @param lanelet A lanelet to check against the + * intersection area. + * @param polygon The polygon to check for containment within the intersection area. + * + * @return bool True if the polygon is within the intersection area, false otherwise. + */ +bool isWithinIntersection( + const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lanelet, + const Polygon2d & polygon); + +/** + * @brief Determines if a polygon is within lanes designated for turning. + * + * Checks if a polygon overlaps with lanelets tagged for turning directions (excluding 'straight'). + * It evaluates the lanelet's 'turn_direction' attribute and determines overlap with the lanelet's + * area. + * + * @param lanelet Lanelet representing the road segment whose turn direction is to be evaluated. + * @param polygon The polygon to be checked for its presence within turn direction lanes. + * + * @return bool True if the polygon is within a lane designated for turning, false if it is within a + * straight lane or no turn direction is specified. + */ +bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon); + +/** + * @brief Calculates the distance required during a lane change operation. + * + * Used for computing prepare or lane change length based on current and maximum velocity, + * acceleration, and duration, returning the lesser of accelerated distance or distance at max + * velocity. + * + * @param velocity The current velocity of the vehicle in meters per second (m/s). + * @param maximum_velocity The maximum velocity the vehicle can reach in meters per second (m/s). + * @param acceleration The acceleration of the vehicle in meters per second squared (m/s^2). + * @param duration The duration of the lane change in seconds (s). + * @return The calculated minimum distance in meters (m). + */ +double calcPhaseLength( + const double velocity, const double maximum_velocity, const double acceleration, + const double time); } // namespace behavior_path_planner::utils::lane_change #endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_lane_change_module/src/manager.cpp index e4405a31a360d..f4fd6fafca0e5 100644 --- a/planning/behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_lane_change_module/src/manager.cpp @@ -64,8 +64,12 @@ void LaneChangeModuleManager::init(rclcpp::Node * node) p.max_longitudinal_acc = getOrDeclareParameter(*node, parameter("max_longitudinal_acc")); // collision check - p.enable_prepare_segment_collision_check = - getOrDeclareParameter(*node, parameter("enable_prepare_segment_collision_check")); + p.enable_collision_check_for_prepare_phase_in_general_lanes = getOrDeclareParameter( + *node, parameter("enable_collision_check_for_prepare_phase.general_lanes")); + p.enable_collision_check_for_prepare_phase_in_intersection = getOrDeclareParameter( + *node, parameter("enable_collision_check_for_prepare_phase.intersection")); + p.enable_collision_check_for_prepare_phase_in_turns = + getOrDeclareParameter(*node, parameter("enable_collision_check_for_prepare_phase.turns")); p.prepare_segment_ignore_object_velocity_thresh = getOrDeclareParameter( *node, parameter("prepare_segment_ignore_object_velocity_thresh")); p.check_objects_on_current_lanes = diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index f0972d66416c8..a8e69df77edc1 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -802,23 +802,27 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects( target_objects.other_lane.reserve(target_obj_index.other_lane.size()); // objects in current lane + const auto is_check_prepare_phase = check_prepare_phase(); for (const auto & obj_idx : target_obj_index.current_lane) { const auto extended_object = utils::lane_change::transform( - objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_); + objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_, + is_check_prepare_phase); target_objects.current_lane.push_back(extended_object); } // objects in target lane for (const auto & obj_idx : target_obj_index.target_lane) { const auto extended_object = utils::lane_change::transform( - objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_); + objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_, + is_check_prepare_phase); target_objects.target_lane.push_back(extended_object); } // objects in other lane for (const auto & obj_idx : target_obj_index.other_lane) { const auto extended_object = utils::lane_change::transform( - objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_); + objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_, + is_check_prepare_phase); target_objects.other_lane.push_back(extended_object); } @@ -878,8 +882,8 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( const auto obj_velocity_norm = std::hypot( object.kinematics.initial_twist_with_covariance.twist.linear.x, object.kinematics.initial_twist_with_covariance.twist.linear.y); - const auto extended_object = - utils::lane_change::transform(object, common_parameters, *lane_change_parameters_); + const auto extended_object = utils::lane_change::transform( + object, common_parameters, *lane_change_parameters_, check_prepare_phase()); const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); @@ -1878,4 +1882,41 @@ void NormalLaneChange::updateStopTime() stop_watch_.tic("stop_time"); } +bool NormalLaneChange::check_prepare_phase() const +{ + const auto & route_handler = getRouteHandler(); + const auto & vehicle_info = getCommonParam().vehicle_info; + + const auto check_in_general_lanes = + lane_change_parameters_->enable_collision_check_for_prepare_phase_in_general_lanes; + + lanelet::ConstLanelet current_lane; + if (!route_handler->getClosestLaneletWithinRoute(getEgoPose(), ¤t_lane)) { + RCLCPP_DEBUG( + logger_, "Unable to get current lane. Default to %s.", + (check_in_general_lanes ? "true" : "false")); + return check_in_general_lanes; + } + + const auto ego_footprint = utils::lane_change::getEgoCurrentFootprint(getEgoPose(), vehicle_info); + + const auto check_in_intersection = std::invoke([&]() { + if (!lane_change_parameters_->enable_collision_check_for_prepare_phase_in_intersection) { + return false; + } + + return utils::lane_change::isWithinIntersection(route_handler, current_lane, ego_footprint); + }); + + const auto check_in_turns = std::invoke([&]() { + if (!lane_change_parameters_->enable_collision_check_for_prepare_phase_in_turns) { + return false; + } + + return utils::lane_change::isWithinTurnDirectionLanes(current_lane, ego_footprint); + }); + + return check_in_intersection || check_in_turns || check_in_general_lanes; +} + } // namespace behavior_path_planner 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 df73990b770a3..60d7c699dc6bb 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -30,7 +30,13 @@ #include #include #include +#include #include +#include + +#include + +#include #include #include @@ -1099,7 +1105,7 @@ std::optional createPolygon( ExtendedPredictedObject transform( const PredictedObject & object, [[maybe_unused]] const BehaviorPathPlannerParameters & common_parameters, - const LaneChangeParameters & lane_change_parameters) + const LaneChangeParameters & lane_change_parameters, const bool check_at_prepare_phase) { ExtendedPredictedObject extended_object; extended_object.uuid = object.object_id; @@ -1109,8 +1115,6 @@ ExtendedPredictedObject transform( extended_object.shape = object.shape; const auto & time_resolution = lane_change_parameters.prediction_time_resolution; - const auto & check_at_prepare_phase = - lane_change_parameters.enable_prepare_segment_collision_check; const auto & prepare_duration = lane_change_parameters.lane_change_prepare_duration; const auto & velocity_threshold = lane_change_parameters.prepare_segment_ignore_object_velocity_thresh; @@ -1168,4 +1172,51 @@ rclcpp::Logger getLogger(const std::string & type) { return rclcpp::get_logger("lane_change").get_child(type); } + +Polygon2d getEgoCurrentFootprint( + const Pose & ego_pose, const vehicle_info_util::VehicleInfo & ego_info) +{ + const auto base_to_front = ego_info.max_longitudinal_offset_m; + const auto base_to_rear = ego_info.rear_overhang_m; + const auto width = ego_info.vehicle_width_m; + + return tier4_autoware_utils::toFootprint(ego_pose, base_to_front, base_to_rear, width); +} + +bool isWithinIntersection( + const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lanelet, + const Polygon2d & polygon) +{ + const std::string id = lanelet.attributeOr("intersection_area", "else"); + if (id == "else") { + return false; + } + + const auto lanelet_polygon = + route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(id.c_str())); + + return boost::geometry::within( + polygon, utils::toPolygon2d(lanelet::utils::to2D(lanelet_polygon.basicPolygon()))); +} + +bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon) +{ + const std::string turn_direction = lanelet.attributeOr("turn_direction", "else"); + if (turn_direction == "else" || turn_direction == "straight") { + return false; + } + + return !boost::geometry::disjoint( + polygon, utils::toPolygon2d(lanelet::utils::to2D(lanelet.polygon2d().basicPolygon()))); +} + +double calcPhaseLength( + const double velocity, const double maximum_velocity, const double acceleration, + const double duration) +{ + const auto length_with_acceleration = + velocity * duration + 0.5 * acceleration * std::pow(duration, 2); + const auto length_with_max_velocity = maximum_velocity * duration; + return std::min(length_with_acceleration, length_with_max_velocity); +} } // namespace behavior_path_planner::utils::lane_change