Skip to content

Commit

Permalink
refactor(lane_change): reducing clang-tidy warnings (autowarefoundati…
Browse files Browse the repository at this point in the history
…on#9085)

* refactor(lane_change): reducing clang-tidy warnings

Signed-off-by: Zulfaqar Azmi <[email protected]>

* change function name to snake case

Signed-off-by: Zulfaqar Azmi <[email protected]>

---------

Signed-off-by: Zulfaqar Azmi <[email protected]>
  • Loading branch information
zulfaqar-azmi-t4 authored Oct 17, 2024
1 parent 37808bf commit 2b9138d
Show file tree
Hide file tree
Showing 5 changed files with 22 additions and 31 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -126,8 +126,8 @@ class NormalLaneChange : public LaneChangeBase

std::vector<double> 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;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -140,7 +140,7 @@ std::vector<PhaseMetrics> calc_prepare_phase_metrics(

std::vector<PhaseMetrics> 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<double>::max());

/**
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -84,8 +84,8 @@ std::vector<int64_t> replaceWithSortedIds(

std::vector<std::vector<int64_t>> 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(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down Expand Up @@ -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<double>::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 =
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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
{
Expand Down Expand Up @@ -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();

Expand Down Expand Up @@ -1667,7 +1666,8 @@ std::optional<LaneChangePath> 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<float>(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,
Expand All @@ -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;

Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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<int>(abort_return_idx));

if (!reference_lane_segment.points.empty()) {
abort_path.path = utils::combinePath(aborting_path, reference_lane_segment);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -52,7 +50,6 @@
#include <iterator>
#include <limits>
#include <memory>
#include <numeric>
#include <optional>
#include <string>
#include <vector>
Expand All @@ -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;
Expand Down Expand Up @@ -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<float>(prepare_velocity));
for (auto & point : prepare_segment.points) {
point.point.longitudinal_velocity_mps =
std::min(point.point.longitudinal_velocity_mps, static_cast<float>(prepare_velocity));
}
} else {
// deceleration
Expand All @@ -142,7 +137,7 @@ std::vector<double> 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<double>(sampling_num);

std::vector<double> sampled_values{min_acc};
for (double sampled_acc = min_acc + resolution;
Expand All @@ -159,7 +154,7 @@ std::vector<double> 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)
{
Expand Down Expand Up @@ -430,7 +425,7 @@ std::vector<DrivableLanes> 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();
Expand Down Expand Up @@ -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;
}
Expand All @@ -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;
}
Expand Down

0 comments on commit 2b9138d

Please sign in to comment.