Skip to content

Commit d1972cd

Browse files
pre-commit-ci[bot]liuXinGangChina
authored andcommitted
style(pre-commit): autofix
1 parent 26a8367 commit d1972cd

File tree

5 files changed

+25
-15
lines changed

5 files changed

+25
-15
lines changed

planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -248,7 +248,8 @@ class VelocitySmootherNode : public rclcpp::Node
248248

249249
TrajectoryPoint calc_projected_trajectory_point(
250250
const TrajectoryPoints & trajectory, const Pose & pose) const;
251-
TrajectoryPoint calc_projected_trajectory_point_from_ego(const TrajectoryPoints & trajectory) const;
251+
TrajectoryPoint calc_projected_trajectory_point_from_ego(
252+
const TrajectoryPoints & trajectory) const;
252253

253254
// parameter handling
254255
void init_common_param();

planning/autoware_velocity_smoother/src/node.cpp

+8-5
Original file line numberDiff line numberDiff line change
@@ -474,7 +474,8 @@ void VelocitySmootherNode::on_current_trajectory(const Trajectory::ConstSharedPt
474474

475475
// calculate prev closest point
476476
if (!prev_output_.empty()) {
477-
current_closest_point_from_prev_output_ = calc_projected_trajectory_point_from_ego(prev_output_);
477+
current_closest_point_from_prev_output_ =
478+
calc_projected_trajectory_point_from_ego(prev_output_);
478479
}
479480

480481
// calculate distance to insert external velocity limit
@@ -750,8 +751,8 @@ void VelocitySmootherNode::insert_behind_velocity(
750751
return autoware::motion_utils::findNearestSegmentIndex(
751752
prev_output_, output.at(i).pose.position);
752753
}();
753-
const auto prev_output_point =
754-
trajectory_utils::calc_interpolated_trajectory_point(prev_output_, output.at(i).pose, seg_idx);
754+
const auto prev_output_point = trajectory_utils::calc_interpolated_trajectory_point(
755+
prev_output_, output.at(i).pose, seg_idx);
755756

756757
// output should be always positive: TODO(Horibe) think better way
757758
output.at(i).longitudinal_velocity_mps =
@@ -819,7 +820,8 @@ std::pair<Motion, VelocitySmootherNode::InitializeType> VelocitySmootherNode::ca
819820
if (stop_dist > node_param_.stop_dist_to_prohibit_engage) {
820821
RCLCPP_DEBUG(
821822
get_logger(),
822-
"calc_initial_motion : vehicle speed is low (%.3f), and desired speed is high (%.3f). Use "
823+
"calc_initial_motion : vehicle speed is low (%.3f), and desired speed is high (%.3f). "
824+
"Use "
823825
"engage speed (%.3f) until vehicle speed reaches engage_vel_thr (%.3f). stop_dist = %.3f",
824826
vehicle_speed, target_vel, node_param_.engage_velocity, engage_vel_thr, stop_dist);
825827
const double engage_acceleration =
@@ -834,7 +836,8 @@ std::pair<Motion, VelocitySmootherNode::InitializeType> VelocitySmootherNode::ca
834836
} else if (target_vel > 0.0) {
835837
RCLCPP_WARN_THROTTLE(
836838
get_logger(), *clock_, 3000,
837-
"calc_initial_motion : target velocity(%.3f[m/s]) is lower than engage velocity(%.3f[m/s]). ",
839+
"calc_initial_motion : target velocity(%.3f[m/s]) is lower than engage "
840+
"velocity(%.3f[m/s]). ",
838841
target_vel, node_param_.engage_velocity);
839842
}
840843
}

planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -315,7 +315,8 @@ TrajectoryPoints AnalyticalJerkConstrainedSmoother::apply_lateral_acceleration_f
315315
static_cast<size_t>(std::max(static_cast<int>((curvature_calc_dist) / points_interval), 1));
316316

317317
// Calculate curvature assuming the trajectory points interval is constant
318-
const auto curvature_v = trajectory_utils::calc_trajectory_curvature_from_3_points(output, idx_dist);
318+
const auto curvature_v =
319+
trajectory_utils::calc_trajectory_curvature_from_3_points(output, idx_dist);
319320

320321
// Decrease speed according to lateral G
321322
const size_t before_decel_index =

planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -77,7 +77,8 @@ bool LinfPseudoJerkSmoother::apply(
7777
return false;
7878
}
7979

80-
std::vector<double> interval_dist_arr = trajectory_utils::calc_trajectory_interval_distance(input);
80+
std::vector<double> interval_dist_arr =
81+
trajectory_utils::calc_trajectory_interval_distance(input);
8182

8283
std::vector<double> v_max(N, 0.0);
8384
for (size_t i = 0; i < N; ++i) {

planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp

+11-7
Original file line numberDiff line numberDiff line change
@@ -54,7 +54,8 @@ TrajectoryPoints applyPreProcess(
5454
arc_length.push_back(s);
5555
}
5656

57-
const auto points = autoware::motion_utils::resampleTrajectory(convertToTrajectory(input), arc_length);
57+
const auto points =
58+
autoware::motion_utils::resampleTrajectory(convertToTrajectory(input), arc_length);
5859
output = convertToTrajectoryPointArray(points);
5960
output.back() = input.back(); // keep the final speed.
6061

@@ -173,7 +174,8 @@ TrajectoryPoints SmootherBase::apply_lateral_acceleration_filter(
173174
std::max(static_cast<int>((base_param_.curvature_calculation_distance) / points_interval), 1));
174175

175176
// Calculate curvature assuming the trajectory points interval is constant
176-
const auto curvature_v = trajectory_utils::calc_trajectory_curvature_from_3_points(output, idx_dist);
177+
const auto curvature_v =
178+
trajectory_utils::calc_trajectory_curvature_from_3_points(output, idx_dist);
177179

178180
// Decrease speed according to lateral G
179181
const size_t before_decel_index =
@@ -183,10 +185,11 @@ TrajectoryPoints SmootherBase::apply_lateral_acceleration_filter(
183185
const double max_lateral_accel_abs = std::fabs(base_param_.max_lateral_accel);
184186

185187
const auto latacc_min_vel_arr =
186-
enable_smooth_limit ? trajectory_utils::calc_velocity_profile_with_constant_jerk_and_acceleration_limit(
187-
output, v0, a0, base_param_.min_jerk, base_param_.max_accel,
188-
base_param_.min_decel_for_lateral_acc_lim_filter)
189-
: std::vector<double>{};
188+
enable_smooth_limit
189+
? trajectory_utils::calc_velocity_profile_with_constant_jerk_and_acceleration_limit(
190+
output, v0, a0, base_param_.min_jerk, base_param_.max_accel,
191+
base_param_.min_decel_for_lateral_acc_lim_filter)
192+
: std::vector<double>{};
190193

191194
for (size_t i = 0; i < output.size(); ++i) {
192195
double curvature = 0.0;
@@ -229,7 +232,8 @@ TrajectoryPoints SmootherBase::apply_steering_rate_limit(
229232
std::max(static_cast<int>((base_param_.curvature_calculation_distance) / points_interval), 1));
230233

231234
// Step1. Calculate curvature assuming the trajectory points interval is constant.
232-
const auto curvature_v = trajectory_utils::calc_trajectory_curvature_from_3_points(output, idx_dist);
235+
const auto curvature_v =
236+
trajectory_utils::calc_trajectory_curvature_from_3_points(output, idx_dist);
233237

234238
// Step2. Calculate steer rate for each trajectory point.
235239
std::vector<double> steer_rate_arr(output.size(), 0.0);

0 commit comments

Comments
 (0)