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] authored and esteve committed Sep 18, 2024
1 parent 7f25fdb commit 58f0d9a
Show file tree
Hide file tree
Showing 13 changed files with 44 additions and 29 deletions.
9 changes: 6 additions & 3 deletions common/autoware_motion_utils/src/resample/resample.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -287,7 +287,8 @@ tier4_planning_msgs::msg::PathWithLaneId resamplePath(
autoware::interpolation::calc_closest_segment_indices(input_arclength, resampling_arclength);

const auto zoh = [&](const auto & input) {
return autoware::interpolation::zero_order_hold(input_arclength, input, closest_segment_indices);
return autoware::interpolation::zero_order_hold(
input_arclength, input, closest_segment_indices);
};

const auto interpolated_pose =
Expand Down Expand Up @@ -471,7 +472,8 @@ autoware_planning_msgs::msg::Path resamplePath(
autoware::interpolation::calc_closest_segment_indices(input_arclength, resampled_arclength);
}
const auto zoh = [&](const auto & input) {
return autoware::interpolation::zero_order_hold(input_arclength, input, closest_segment_indices);
return autoware::interpolation::zero_order_hold(
input_arclength, input, closest_segment_indices);
};

const auto interpolated_pose =
Expand Down Expand Up @@ -645,7 +647,8 @@ autoware_planning_msgs::msg::Trajectory resampleTrajectory(
autoware::interpolation::calc_closest_segment_indices(input_arclength, resampled_arclength);
}
const auto zoh = [&](const auto & input) {
return autoware::interpolation::zero_order_hold(input_arclength, input, closest_segment_indices);
return autoware::interpolation::zero_order_hold(
input_arclength, input, closest_segment_indices);
};

const auto interpolated_pose =
Expand Down
12 changes: 6 additions & 6 deletions common/autoware_motion_utils/src/trajectory/interpolation.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -70,19 +70,19 @@ TrajectoryPoint calcInterpolatedPoint(
curr_pt.longitudinal_velocity_mps, next_pt.longitudinal_velocity_mps, clamped_ratio);
interpolated_point.lateral_velocity_mps = autoware::interpolation::lerp(
curr_pt.lateral_velocity_mps, next_pt.lateral_velocity_mps, clamped_ratio);
interpolated_point.acceleration_mps2 =
autoware::interpolation::lerp(curr_pt.acceleration_mps2, next_pt.acceleration_mps2, clamped_ratio);
interpolated_point.acceleration_mps2 = autoware::interpolation::lerp(
curr_pt.acceleration_mps2, next_pt.acceleration_mps2, clamped_ratio);
}

// heading rate interpolation
interpolated_point.heading_rate_rps =
autoware::interpolation::lerp(curr_pt.heading_rate_rps, next_pt.heading_rate_rps, clamped_ratio);
interpolated_point.heading_rate_rps = autoware::interpolation::lerp(
curr_pt.heading_rate_rps, next_pt.heading_rate_rps, clamped_ratio);

// wheel interpolation
interpolated_point.front_wheel_angle_rad = autoware::interpolation::lerp(
curr_pt.front_wheel_angle_rad, next_pt.front_wheel_angle_rad, clamped_ratio);
interpolated_point.rear_wheel_angle_rad =
autoware::interpolation::lerp(curr_pt.rear_wheel_angle_rad, next_pt.rear_wheel_angle_rad, clamped_ratio);
interpolated_point.rear_wheel_angle_rad = autoware::interpolation::lerp(
curr_pt.rear_wheel_angle_rad, next_pt.rear_wheel_angle_rad, clamped_ratio);

// time interpolation
const double interpolated_time = autoware::interpolation::lerp(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -179,7 +179,8 @@ geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance(
p.position.x = autoware::interpolation::lerp(p0.position.x, p1.position.x, ratio);
p.position.y = autoware::interpolation::lerp(p0.position.y, p1.position.y, ratio);
p.position.z = autoware::interpolation::lerp(p0.position.z, p1.position.z, ratio);
p.orientation = autoware::interpolation::lerpOrientation(p0.orientation, p1.orientation, ratio);
p.orientation =
autoware::interpolation::lerpOrientation(p0.orientation, p1.orientation, ratio);
break;
}
remain_dist -= dist;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -123,19 +123,19 @@ TrajectoryPoint calcInterpolatedPoint(
curr_pt.longitudinal_velocity_mps, next_pt.longitudinal_velocity_mps, clamped_ratio);
interpolated_point.lateral_velocity_mps = autoware::interpolation::lerp(
curr_pt.lateral_velocity_mps, next_pt.lateral_velocity_mps, clamped_ratio);
interpolated_point.acceleration_mps2 =
autoware::interpolation::lerp(curr_pt.acceleration_mps2, next_pt.acceleration_mps2, clamped_ratio);
interpolated_point.acceleration_mps2 = autoware::interpolation::lerp(
curr_pt.acceleration_mps2, next_pt.acceleration_mps2, clamped_ratio);
}

// heading rate interpolation
interpolated_point.heading_rate_rps =
autoware::interpolation::lerp(curr_pt.heading_rate_rps, next_pt.heading_rate_rps, clamped_ratio);
interpolated_point.heading_rate_rps = autoware::interpolation::lerp(
curr_pt.heading_rate_rps, next_pt.heading_rate_rps, clamped_ratio);

// wheel interpolation
interpolated_point.front_wheel_angle_rad = autoware::interpolation::lerp(
curr_pt.front_wheel_angle_rad, next_pt.front_wheel_angle_rad, clamped_ratio);
interpolated_point.rear_wheel_angle_rad =
autoware::interpolation::lerp(curr_pt.rear_wheel_angle_rad, next_pt.rear_wheel_angle_rad, clamped_ratio);
interpolated_point.rear_wheel_angle_rad = autoware::interpolation::lerp(
curr_pt.rear_wheel_angle_rad, next_pt.rear_wheel_angle_rad, clamped_ratio);

// time interpolation
const double interpolated_time = autoware::interpolation::lerp(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -234,8 +234,10 @@ bool calcStopVelocityWithConstantJerkAccLimit(
}

if (
!autoware::interpolation::isIncreasing(xs) || !autoware::interpolation::isIncreasing(distances) ||
!autoware::interpolation::isNotDecreasing(xs) || !autoware::interpolation::isNotDecreasing(distances)) {
!autoware::interpolation::isIncreasing(xs) ||
!autoware::interpolation::isIncreasing(distances) ||
!autoware::interpolation::isNotDecreasing(xs) ||
!autoware::interpolation::isNotDecreasing(distances)) {
return false;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -108,7 +108,8 @@ inline Point2d normal_at_distance(const Point2d & p1, const Point2d & p2, const
/// @return point interpolated between a and b as per the given ratio
inline Point2d lerp_point(const Point2d & a, const Point2d & b, const double ratio)
{
return {interpolation::lerp(a.x(), b.x(), ratio), autoware::interpolation::lerp(a.y(), b.y(), ratio)};
return {
interpolation::lerp(a.x(), b.x(), ratio), autoware::interpolation::lerp(a.y(), b.y(), ratio)};
}

/// @brief calculate the point with distance and arc length relative to a linestring
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -284,8 +284,10 @@ struct Trajectory : Path
t.curvatures = autoware::interpolation::lerp(times, curvatures, t.times);
t.jerks = autoware::interpolation::lerp(times, jerks, t.times);
t.yaws = autoware::interpolation::lerp(times, yaws, t.times);
t.longitudinal_velocities = autoware::interpolation::lerp(times, longitudinal_velocities, t.times);
t.longitudinal_accelerations = autoware::interpolation::lerp(times, longitudinal_accelerations, t.times);
t.longitudinal_velocities =
autoware::interpolation::lerp(times, longitudinal_velocities, t.times);
t.longitudinal_accelerations =
autoware::interpolation::lerp(times, longitudinal_accelerations, t.times);
t.lateral_velocities = autoware::interpolation::lerp(times, lateral_velocities, t.times);
t.lateral_accelerations = autoware::interpolation::lerp(times, lateral_accelerations, t.times);
t.constraint_results = constraint_results;
Expand Down Expand Up @@ -320,8 +322,10 @@ struct Trajectory : Path
t.curvatures = autoware::interpolation::lerp(times, curvatures, t.times);
t.jerks = autoware::interpolation::lerp(times, jerks, t.times);
t.yaws = autoware::interpolation::lerp(times, yaws, t.times);
t.longitudinal_velocities = autoware::interpolation::lerp(times, longitudinal_velocities, t.times);
t.longitudinal_accelerations = autoware::interpolation::lerp(times, longitudinal_accelerations, t.times);
t.longitudinal_velocities =
autoware::interpolation::lerp(times, longitudinal_velocities, t.times);
t.longitudinal_accelerations =
autoware::interpolation::lerp(times, longitudinal_accelerations, t.times);
t.lateral_velocities = autoware::interpolation::lerp(times, lateral_velocities, t.times);
t.lateral_accelerations = autoware::interpolation::lerp(times, lateral_accelerations, t.times);
t.constraint_results = constraint_results;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,6 @@
#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_ACTUATION_CMD_HPP_

#include "autoware/interpolation/linear_interpolation.hpp"

#include "eigen3/Eigen/Core"
#include "eigen3/Eigen/LU"
#include "simple_planning_simulator/utils/csv_loader.hpp"
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,8 @@ class AccelerationMap

// (throttle, vel, acc) map => (throttle, acc) map by fixing vel
for (const auto & acc_vec : acceleration_map_) {
interpolated_acc_vec.push_back(autoware::interpolation::lerp(vel_index_, acc_vec, clamped_vel));
interpolated_acc_vec.push_back(
autoware::interpolation::lerp(vel_index_, acc_vec, clamped_vel));
}
// calculate throttle
// When the desired acceleration is smaller than the throttle area, return min acc
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,8 @@ double ActuationMap::getControlCommand(const double actuation, const double stat
}

const double clamped_actuation = CSVLoader::clampValue(actuation, actuation_index_);
return autoware::interpolation::lerp(actuation_index_, interpolated_control_vec, clamped_actuation);
return autoware::interpolation::lerp(
actuation_index_, interpolated_control_vec, clamped_actuation);
}

std::optional<double> AccelMap::getThrottle(const double acc, double vel) const
Expand Down
3 changes: 2 additions & 1 deletion vehicle/autoware_raw_vehicle_cmd_converter/src/accel_map.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,8 @@ bool AccelMap::getThrottle(const double acc, double vel, double & throttle) cons
const double clamped_vel = CSVLoader::clampValue(vel, vel_index_, "throttle: vel");
// (throttle, vel, acc) map => (throttle, acc) map by fixing vel
for (std::vector<double> accelerations : accel_map_) {
interpolated_acc_vec.push_back(autoware::interpolation::lerp(vel_index_, accelerations, clamped_vel));
interpolated_acc_vec.push_back(
autoware::interpolation::lerp(vel_index_, accelerations, clamped_vel));
}
// calculate throttle
// When the desired acceleration is smaller than the throttle area, return false => brake sequence
Expand Down
3 changes: 2 additions & 1 deletion vehicle/autoware_raw_vehicle_cmd_converter/src/brake_map.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,8 @@ bool BrakeMap::getBrake(const double acc, const double vel, double & brake)

// (throttle, vel, acc) map => (throttle, acc) map by fixing vel
for (std::vector<double> accelerations : brake_map_) {
interpolated_acc_vec.push_back(autoware::interpolation::lerp(vel_index_, accelerations, clamped_vel));
interpolated_acc_vec.push_back(
autoware::interpolation::lerp(vel_index_, accelerations, clamped_vel));
}

// calculate brake
Expand Down
3 changes: 2 additions & 1 deletion vehicle/autoware_raw_vehicle_cmd_converter/src/steer_map.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,8 @@ void SteerMap::getSteer(const double steer_rate, const double steer, double & ou
const double clamped_steer = CSVLoader::clampValue(steer, steer_index_, "steer: steer");
std::vector<double> steer_rate_interp = {};
for (const auto & steer_rate_vec : steer_map_) {
steer_rate_interp.push_back(autoware::interpolation::lerp(steer_index_, steer_rate_vec, clamped_steer));
steer_rate_interp.push_back(
autoware::interpolation::lerp(steer_index_, steer_rate_vec, clamped_steer));
}

const double clamped_steer_rate =
Expand Down

0 comments on commit 58f0d9a

Please sign in to comment.