Skip to content

Commit

Permalink
fix format
Browse files Browse the repository at this point in the history
Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>
  • Loading branch information
TakaHoribe committed Aug 20, 2023
1 parent 487387f commit 41a2cd6
Showing 1 changed file with 8 additions and 10 deletions.
18 changes: 8 additions & 10 deletions control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -62,13 +62,15 @@ double calcLatAcc(const AckermannControlCommand & cmd, const double wheelbase)
return v * v * std::tan(cmd.lateral.steering_tire_angle) / wheelbase;
}

double calcLatJerk(const AckermannControlCommand & cmd, const AckermannControlCommand & prev_cmd, const double wheelbase, const double dt)
double calcLatJerk(
const AckermannControlCommand & cmd, const AckermannControlCommand & prev_cmd,
const double wheelbase, const double dt)
{
const auto prev_v = prev_cmd.longitudinal.speed;
const auto prev = prev_v * prev_v * std::tan(prev_cmd.lateral.steering_tire_angle) / wheelbase;
const auto prev = prev_v * prev_v * std::tan(prev_cmd.lateral.steering_tire_angle) / wheelbase;

const auto curr_v = cmd.longitudinal.speed;
const auto curr = curr_v * curr_v * std::tan(cmd.lateral.steering_tire_angle) / wheelbase;
const auto curr = curr_v * curr_v * std::tan(cmd.lateral.steering_tire_angle) / wheelbase;

return (curr - prev) / dt;
}
Expand Down Expand Up @@ -235,7 +237,6 @@ TEST(VehicleCmdFilter, VehicleCmdFilter)
}
}


TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate)
{
constexpr double WHEELBASE = 2.8;
Expand Down Expand Up @@ -370,9 +371,7 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate)
// lateral acc lim
// p.reference_speed_points = std::vector<double>{2.0, 4.0, 10.0};
// p.lat_acc_lim = std::vector<double>{0.1, 0.2, 0.3};
const auto _calcLatAcc = [&](const auto & cmd){
return calcLatAcc(cmd, WHEELBASE);
};
const auto _calcLatAcc = [&](const auto & cmd) { return calcLatAcc(cmd, WHEELBASE); };
{
set_speed_and_reset_prev(0.0);
EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.1, ep);
Expand All @@ -399,7 +398,7 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate)
// lateral jerk lim
// p.reference_speed_points = std::vector<double>{2.0, 4.0, 10.0};
// p.lat_jerk_lim = std::vector<double>{0.9, 0.7, 0.1};
const auto _calcLatJerk = [&](const auto & cmd){
const auto _calcLatJerk = [&](const auto & cmd) {
return calcLatJerk(cmd, AckermannControlCommand{}, WHEELBASE, DT);
};
{
Expand Down Expand Up @@ -434,7 +433,6 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate)
EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * 0.1, ep);
}


// steering diff lim
// p.reference_speed_points = std::vector<double>{2.0, 4.0, 10.0};
// p.actual_steer_diff_lim = std::vector<double>{0.1, 0.3, 0.2};
Expand Down Expand Up @@ -462,4 +460,4 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate)
set_speed_and_reset_prev(15.0);
EXPECT_NEAR((_limitActualSteerDiff(orig_cmd).lateral.steering_tire_angle), 0.2, ep);
}
}
}

0 comments on commit 41a2cd6

Please sign in to comment.