Skip to content

Commit

Permalink
style(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
pre-commit-ci[bot] committed Jul 16, 2023
1 parent e8d5d6f commit b9ab425
Showing 1 changed file with 6 additions and 8 deletions.
14 changes: 6 additions & 8 deletions planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -35,8 +35,7 @@ std::tuple<std::vector<double>, std::vector<double>> calcVehicleCirclesByUniform
const vehicle_info_util::VehicleInfo & vehicle_info, const size_t circle_num,
const double radius_ratio)
{
const double lateral_offset =
abs(vehicle_info.right_overhang_m - vehicle_info.left_overhang_m);
const double lateral_offset = abs(vehicle_info.right_overhang_m - vehicle_info.left_overhang_m);

const double radius = std::hypot(
vehicle_info.vehicle_length_m / static_cast<double>(circle_num) / 2.0,
Expand All @@ -62,10 +61,10 @@ std::tuple<std::vector<double>, std::vector<double>> calcVehicleCirclesByBicycle
if (circle_num < 2) {
throw std::invalid_argument("circle_num is less than 2.");
}
const double lateral_offset =
abs(vehicle_info.right_overhang_m - vehicle_info.left_overhang_m);
const double lateral_offset = abs(vehicle_info.right_overhang_m - vehicle_info.left_overhang_m);
// 1st circle (rear wheel)
const double rear_radius = ((vehicle_info.vehicle_width_m / 2.0) + lateral_offset) * rear_radius_ratio;
const double rear_radius =
((vehicle_info.vehicle_width_m / 2.0) + lateral_offset) * rear_radius_ratio;
const double rear_lon_offset = 0.0;

// 2nd circle (front wheel)
Expand All @@ -88,8 +87,7 @@ std::tuple<std::vector<double>, std::vector<double>> calcVehicleCirclesByFitting
if (circle_num < 2) {
throw std::invalid_argument("circle_num is less than 2.");
}
const double lateral_offset =
abs(vehicle_info.right_overhang_m - vehicle_info.left_overhang_m);
const double lateral_offset = abs(vehicle_info.right_overhang_m - vehicle_info.left_overhang_m);
const double radius = vehicle_info.vehicle_width_m / 2.0 + lateral_offset;
std::vector<double> radiuses(circle_num, radius);

Expand Down Expand Up @@ -1383,7 +1381,7 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix(
const Eigen::VectorXd CW = C_sparse_mat * mpt_mat.W + C_vec;

// calculate bounds
[[maybe_unused]] const double bounds_offset =
[[maybe_unused]] const double bounds_offset =
vehicle_info_.vehicle_width_m / 2.0 - vehicle_circle_radiuses_.at(l_idx);
const auto & [part_ub, part_lb] = extractBounds(ref_points, l_idx, vehicle_info_);

Expand Down

0 comments on commit b9ab425

Please sign in to comment.