diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index 3b414235a9f54..4dec256a65703 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -126,8 +126,8 @@ class NormalLaneChange : public LaneChangeBase std::vector calc_prepare_durations() const; - lane_change::TargetObjects getTargetObjects( - const FilteredByLanesExtendedObjects & predicted_objects, + lane_change::TargetObjects get_target_objects( + const FilteredByLanesExtendedObjects & filtered_objects, const lanelet::ConstLanelets & current_lanes) const; FilteredByLanesExtendedObjects filterObjects() const; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp index 021fa16fa6ec1..233fc886f836f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp @@ -140,7 +140,7 @@ std::vector calc_prepare_phase_metrics( std::vector calc_shift_phase_metrics( const CommonDataPtr & common_data_ptr, const double shift_length, const double initial_velocity, - const double max_velocity, const double lon_accel, + const double max_path_velocity, const double lon_accel, const double max_length_threshold = std::numeric_limits::max()); /** 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 729eaa5ca8268..b6c234fd7035d 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 @@ -84,8 +84,8 @@ std::vector replaceWithSortedIds( std::vector> get_sorted_lane_ids(const CommonDataPtr & common_data_ptr); -lanelet::ConstLanelets getTargetNeighborLanes( - const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, +lanelet::ConstLanelets get_target_neighbor_lanes( + const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const LaneChangeModuleType & type); bool isPathInLanelets( 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 5961435085346..6f71ffff734a0 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 @@ -46,7 +46,6 @@ namespace autoware::behavior_path_planner using autoware::motion_utils::calcSignedArcLength; using utils::lane_change::create_lanes_polygon; using utils::path_safety_checker::isPolygonOverlapLanelet; -using utils::traffic_light::getDistanceToNextTrafficLight; namespace calculation = utils::lane_change::calculation; NormalLaneChange::NormalLaneChange( @@ -91,7 +90,7 @@ void NormalLaneChange::update_lanes(const bool is_approved) common_data_ptr_->current_lanes_path = route_handler_ptr->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); - common_data_ptr_->lanes_ptr->target_neighbor = utils::lane_change::getTargetNeighborLanes( + common_data_ptr_->lanes_ptr->target_neighbor = utils::lane_change::get_target_neighbor_lanes( *route_handler_ptr, current_lanes, common_data_ptr_->lc_type); common_data_ptr_->lanes_ptr->current_lane_in_goal_section = @@ -560,7 +559,7 @@ PathWithLaneId NormalLaneChange::getReferencePath() const get_target_lanes(), getEgoPose(), &closest_lanelet)) { return prev_module_output_.reference_path; } - const auto reference_path = utils::getCenterLinePathFromLanelet(closest_lanelet, planner_data_); + auto reference_path = utils::getCenterLinePathFromLanelet(closest_lanelet, planner_data_); if (reference_path.points.empty()) { return prev_module_output_.reference_path; } @@ -1014,7 +1013,7 @@ bool NormalLaneChange::get_prepare_segment( return true; } -lane_change::TargetObjects NormalLaneChange::getTargetObjects( +lane_change::TargetObjects NormalLaneChange::get_target_objects( const FilteredByLanesExtendedObjects & filtered_objects, [[maybe_unused]] const lanelet::ConstLanelets & current_lanes) const { @@ -1365,7 +1364,7 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const auto current_velocity = getEgoVelocity(); const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); - const auto target_objects = getTargetObjects(filtered_objects_, current_lanes); + const auto target_objects = get_target_objects(filtered_objects_, current_lanes); const auto prepare_phase_metrics = get_prepare_metrics(); @@ -1667,7 +1666,8 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( utils::clipPathLength(reference_segment, 0, length_to_lane_changing_start, 0.0); // remove terminal points because utils::clipPathLength() calculates extra long path reference_segment.points.pop_back(); - reference_segment.points.back().point.longitudinal_velocity_mps = minimum_lane_changing_velocity; + reference_segment.points.back().point.longitudinal_velocity_mps = + static_cast(minimum_lane_changing_velocity); const auto terminal_lane_change_path = utils::lane_change::construct_candidate_path( common_data_ptr_, lane_change_info, reference_segment, target_lane_reference_path, @@ -1686,7 +1686,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const return {true, true}; } - const auto target_objects = getTargetObjects(filtered_objects_, current_lanes); + const auto target_objects = get_target_objects(filtered_objects_, current_lanes); CollisionCheckDebugMap debug_data; @@ -1773,11 +1773,7 @@ bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const } // check relative angle - if (!utils::checkPathRelativeAngle(path, M_PI)) { - return false; - } - - return true; + return utils::checkPathRelativeAngle(path, M_PI); } bool NormalLaneChange::isRequiredStop(const bool is_trailing_object) @@ -1921,7 +1917,7 @@ bool NormalLaneChange::calcAbortPath() PathWithLaneId aborting_path; aborting_path.points.insert( aborting_path.points.begin(), shifted_path.path.points.begin(), - shifted_path.path.points.begin() + abort_return_idx); + shifted_path.path.points.begin() + static_cast(abort_return_idx)); if (!reference_lane_segment.points.empty()) { abort_path.path = utils::combinePath(aborting_path, reference_lane_segment); 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 82a832d30c0b5..b8d34afeaab54 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 @@ -14,10 +14,8 @@ #include "autoware/behavior_path_lane_change_module/utils/utils.hpp" -#include "autoware/behavior_path_lane_change_module/utils/calculation.hpp" #include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" #include "autoware/behavior_path_lane_change_module/utils/path.hpp" -#include "autoware/behavior_path_planner_common/marker_utils/utils.hpp" #include "autoware/behavior_path_planner_common/parameters.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -52,7 +50,6 @@ #include #include #include -#include #include #include #include @@ -61,7 +58,6 @@ namespace autoware::behavior_path_planner::utils::lane_change { using autoware::route_handler::RouteHandler; using autoware::universe_utils::LineString2d; -using autoware::universe_utils::Point2d; using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObjects; @@ -117,10 +113,9 @@ void setPrepareVelocity( { if (current_velocity < prepare_velocity) { // acceleration - for (size_t i = 0; i < prepare_segment.points.size(); ++i) { - prepare_segment.points.at(i).point.longitudinal_velocity_mps = std::min( - prepare_segment.points.at(i).point.longitudinal_velocity_mps, - static_cast(prepare_velocity)); + for (auto & point : prepare_segment.points) { + point.point.longitudinal_velocity_mps = + std::min(point.point.longitudinal_velocity_mps, static_cast(prepare_velocity)); } } else { // deceleration @@ -142,7 +137,7 @@ std::vector getAccelerationValues( } constexpr double epsilon = 0.001; - const auto resolution = std::abs(max_acc - min_acc) / sampling_num; + const auto resolution = std::abs(max_acc - min_acc) / static_cast(sampling_num); std::vector sampled_values{min_acc}; for (double sampled_acc = min_acc + resolution; @@ -159,7 +154,7 @@ std::vector getAccelerationValues( return sampled_values; } -lanelet::ConstLanelets getTargetNeighborLanes( +lanelet::ConstLanelets get_target_neighbor_lanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const LaneChangeModuleType & type) { @@ -430,7 +425,7 @@ std::vector generateDrivableLanes( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & lane_change_lanes) { const auto has_same_lane = - [](const lanelet::ConstLanelets lanes, const lanelet::ConstLanelet & lane) { + [](const lanelet::ConstLanelets & lanes, const lanelet::ConstLanelet & lane) { if (lanes.empty()) return false; const auto has_same = [&](const auto & ll) { return ll.id() == lane.id(); }; return std::find_if(lanes.begin(), lanes.end(), has_same) != lanes.end(); @@ -752,7 +747,7 @@ bool isParkedObject( most_left_lanelet.attribute(lanelet::AttributeName::Subtype); for (const auto & ll : most_left_lanelet_candidates) { - const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype); + const auto & sub_type = ll.attribute(lanelet::AttributeName::Subtype); if (sub_type.value() == "road_shoulder") { most_left_lanelet = ll; } @@ -773,7 +768,7 @@ bool isParkedObject( most_right_lanelet.attribute(lanelet::AttributeName::Subtype); for (const auto & ll : most_right_lanelet_candidates) { - const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype); + const auto & sub_type = ll.attribute(lanelet::AttributeName::Subtype); if (sub_type.value() == "road_shoulder") { most_right_lanelet = ll; }