Skip to content

Commit

Permalink
use reference arg
Browse files Browse the repository at this point in the history 
Signed-off-by: Takamasa Horibe <[email protected]>
  • Loading branch information
@TakaHoribe
TakaHoribe committed Jan 17, 2023
1 parent f36c782 commit e81c91b
Show file tree
Hide file tree
Showing 3 changed files with 57 additions and 50 deletions.
14 changes: 7 additions & 7 deletions planning/planning_validator/include/planning_validator/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,19 +36,19 @@ std::pair<double, size_t> getAbsMaxValAndIdx(const std::vector<double> & v);

Trajectory resampleTrajectory(const Trajectory & trajectory, const double min_interval);

std::vector<double> calcCurvature(const Trajectory & trajectory);
void calcCurvature(const Trajectory & trajectory, std::vector<double> & v);

std::vector<double> calcIntervalDistance(const Trajectory & trajectory);
void calcIntervalDistance(const Trajectory & trajectory, std::vector<double> & v);

std::vector<double> calcLateralAcceleration(const Trajectory & trajectory);
void calcLateralAcceleration(const Trajectory & trajectory, std::vector<double> & v);

std::vector<double> getLongitudinalAccArray(const Trajectory & trajectory);
void getLongitudinalAccArray(const Trajectory & trajectory, std::vector<double> & v);

std::vector<double> calcRelativeAngles(const Trajectory & trajectory);
void calcRelativeAngles(const Trajectory & trajectory, std::vector<double> & v);

std::vector<double> calcSteeringAngles(const Trajectory & trajectory, const double wheelbase);
void calcSteeringAngles(const Trajectory & trajectory, const double wheelbase, std::vector<double> & v);

std::vector<double> calcSteeringRates(const Trajectory & trajectory, const double wheelbase);
void calcSteeringRates(const Trajectory & trajectory, const double wheelbase, std::vector<double> & v);

bool checkFinite(const TrajectoryPoint & point);

Expand Down
24 changes: 16 additions & 8 deletions planning/planning_validator/src/planning_validator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -246,7 +246,8 @@ bool PlanningValidator::checkValidFiniteValue(const Trajectory & trajectory)

bool PlanningValidator::checkValidInterval(const Trajectory & trajectory)
{
const auto interval_distances = calcIntervalDistance(trajectory);
std::vector<double> interval_distances;
calcIntervalDistance(trajectory, interval_distances);
const auto [max_interval_distance, i] = getAbsMaxValAndIdx(interval_distances);
validation_status_.max_interval_distance = max_interval_distance;

Expand All @@ -264,7 +265,8 @@ bool PlanningValidator::checkValidInterval(const Trajectory & trajectory)

bool PlanningValidator::checkValidRelativeAngle(const Trajectory & trajectory)
{
const auto relative_angles = calcRelativeAngles(trajectory);
std::vector<double> relative_angles;
calcRelativeAngles(trajectory, relative_angles);
const auto [max_relative_angle, i] = getAbsMaxValAndIdx(relative_angles);
validation_status_.max_relative_angle = max_relative_angle;

Expand All @@ -282,7 +284,8 @@ bool PlanningValidator::checkValidRelativeAngle(const Trajectory & trajectory)

bool PlanningValidator::checkValidCurvature(const Trajectory & trajectory)
{
const auto curvatures = calcCurvature(trajectory);
std::vector<double> curvatures;
calcCurvature(trajectory, curvatures);
const auto [max_curvature, i] = getAbsMaxValAndIdx(curvatures);
validation_status_.max_curvature = max_curvature;
if (max_curvature > validation_params_.curvature_threshold) {
Expand All @@ -299,7 +302,8 @@ bool PlanningValidator::checkValidCurvature(const Trajectory & trajectory)

bool PlanningValidator::checkValidLateralAcceleration(const Trajectory & trajectory)
{
const auto lateral_acc_arr = calcLateralAcceleration(trajectory);
std::vector<double> lateral_acc_arr;
calcLateralAcceleration(trajectory, lateral_acc_arr);
const auto [max_lateral_acc, i] = getAbsMaxValAndIdx(lateral_acc_arr);
validation_status_.max_lateral_acc = max_lateral_acc;
if (max_lateral_acc > validation_params_.lateral_acc_threshold) {
Expand All @@ -311,7 +315,8 @@ bool PlanningValidator::checkValidLateralAcceleration(const Trajectory & traject

bool PlanningValidator::checkValidMinLongitudinalAcceleration(const Trajectory & trajectory)
{
const auto acc_arr = getLongitudinalAccArray(trajectory);
std::vector<double> acc_arr;
getLongitudinalAccArray(trajectory, acc_arr);
const auto [min_longitudinal_acc, i] = getMinValAndIdx(acc_arr);
validation_status_.min_longitudinal_acc = min_longitudinal_acc;

Expand All @@ -324,7 +329,8 @@ bool PlanningValidator::checkValidMinLongitudinalAcceleration(const Trajectory &

bool PlanningValidator::checkValidMaxLongitudinalAcceleration(const Trajectory & trajectory)
{
const auto acc_arr = getLongitudinalAccArray(trajectory);
std::vector<double> acc_arr;
getLongitudinalAccArray(trajectory, acc_arr);
const auto [max_longitudinal_acc, i] = getAbsMaxValAndIdx(acc_arr);
validation_status_.max_longitudinal_acc = max_longitudinal_acc;

Expand All @@ -337,7 +343,8 @@ bool PlanningValidator::checkValidMaxLongitudinalAcceleration(const Trajectory &

bool PlanningValidator::checkValidSteering(const Trajectory & trajectory)
{
const auto steerings = calcSteeringAngles(trajectory, vehicle_info_.wheel_base_m);
std::vector<double> steerings;
calcSteeringAngles(trajectory, vehicle_info_.wheel_base_m, steerings);
const auto [max_steering, i] = getAbsMaxValAndIdx(steerings);
validation_status_.max_steering = max_steering;

Expand All @@ -350,7 +357,8 @@ bool PlanningValidator::checkValidSteering(const Trajectory & trajectory)

bool PlanningValidator::checkValidSteeringRate(const Trajectory & trajectory)
{
const auto steering_rates = calcSteeringRates(trajectory, vehicle_info_.wheel_base_m);
std::vector<double> steering_rates;
calcSteeringRates(trajectory, vehicle_info_.wheel_base_m, steering_rates);
const auto [max_steering_rate, i] = getAbsMaxValAndIdx(steering_rates);
validation_status_.max_steering_rate = max_steering_rate;

Expand Down
69 changes: 34 additions & 35 deletions planning/planning_validator/src/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,74 +71,71 @@ Trajectory resampleTrajectory(const Trajectory & trajectory, const double min_in
return resampled;
}

std::vector<double> calcCurvature(const Trajectory & trajectory)
void calcCurvature(const Trajectory & trajectory, std::vector<double> &v)
{
if (trajectory.points.size() < 3) {
return std::vector<double>(trajectory.points.size(), 0.0);
v = std::vector<double>(trajectory.points.size(), 0.0);
return;
}

std::vector<double> curvature_arr;
curvature_arr.push_back(0.0);
v.push_back(0.0);
for (size_t i = 1; i < trajectory.points.size() - 1; ++i) {
const auto p1 = getPoint(trajectory.points.at(i - 1));
const auto p2 = getPoint(trajectory.points.at(i));
const auto p3 = getPoint(trajectory.points.at(i + 1));
try {
curvature_arr.push_back(tier4_autoware_utils::calcCurvature(p1, p2, p3));
v.push_back(tier4_autoware_utils::calcCurvature(p1, p2, p3));
} catch (...) {
// maybe distance is too close
curvature_arr.push_back(0.0);
v.push_back(0.0);
}
}
curvature_arr.push_back(0.0);
return curvature_arr;
v.push_back(0.0);
return;
}

std::vector<double> calcIntervalDistance(const Trajectory & trajectory)
void calcIntervalDistance(const Trajectory & trajectory, std::vector<double> & v)
{
if (trajectory.points.size() < 2) {
return std::vector<double>(trajectory.points.size(), 0.0);
v = std::vector<double>(trajectory.points.size(), 0.0);
return;
}

std::vector<double> interval_distances;
for (size_t i = 1; i < trajectory.points.size(); ++i) {
interval_distances.push_back(
v.push_back(
calcDistance2d(trajectory.points.at(i), trajectory.points.at(i - 1)));
}
return interval_distances;
return;
}

std::vector<double> calcLateralAcceleration(const Trajectory & trajectory)
void calcLateralAcceleration(const Trajectory & trajectory, std::vector<double> & lat_acc_arr)
{
const auto curvatures = calcCurvature(trajectory);
std::vector<double> curvatures;
calcCurvature(trajectory, curvatures);

std::vector<double> lat_acc_arr;
for (size_t i = 0; i < curvatures.size(); ++i) {
const auto v = trajectory.points.at(i).longitudinal_velocity_mps;
const auto lat_acc = v * v * curvatures.at(i);
lat_acc_arr.push_back(lat_acc);
}
return lat_acc_arr;
}

std::vector<double> getLongitudinalAccArray(const Trajectory & trajectory)
void getLongitudinalAccArray(const Trajectory & trajectory, std::vector<double> & v)
{
std::vector<double> acc_arr;
for (const auto & p : trajectory.points) {
acc_arr.push_back(p.acceleration_mps2);
v.push_back(p.acceleration_mps2);
}
return acc_arr;
}

std::vector<double> calcRelativeAngles(const Trajectory & trajectory)
void calcRelativeAngles(const Trajectory & trajectory, std::vector<double> & v)
{
// We need at least three points to compute relative angle
const size_t relative_angle_points_num = 3;
if (trajectory.points.size() < relative_angle_points_num) {
return {0.0};
v = {0.0};
return;
}

std::vector<double> relative_angles;

for (size_t i = 0; i <= trajectory.points.size() - relative_angle_points_num; ++i) {
const auto & p1 = trajectory.points.at(i).pose.position;
Expand All @@ -151,36 +148,39 @@ std::vector<double> calcRelativeAngles(const Trajectory & trajectory)
// convert relative angle to [-pi ~ pi]
const auto relative_angle = std::abs(tier4_autoware_utils::normalizeRadian(angle_b - angle_a));

relative_angles.push_back(relative_angle);
v.push_back(relative_angle);
}

return relative_angles;
return;
}

std::vector<double> calcSteeringAngles(const Trajectory & trajectory, const double wheelbase)
void calcSteeringAngles(const Trajectory & trajectory, const double wheelbase, std::vector<double> & v)
{
const auto curvature_to_steering = [](const auto k, const auto wheelbase) {
return std::atan(k * wheelbase);
};

const auto curvatures = calcCurvature(trajectory);
std::vector<double> curvatures;
calcCurvature(trajectory, curvatures);

std::vector<double> steerings;
for (const auto & k : curvatures) {
steerings.push_back(curvature_to_steering(k, wheelbase));
v.push_back(curvature_to_steering(k, wheelbase));
}
return steerings;
return;
}

std::vector<double> calcSteeringRates(const Trajectory & trajectory, const double wheelbase)
void calcSteeringRates(const Trajectory & trajectory, const double wheelbase, std::vector<double> & steering_rates)
{
if (trajectory.points.size() < 1) {
return {0.0};
steering_rates = {0.0};
return;
}

const auto steerings = calcSteeringAngles(trajectory, wheelbase);
std::vector<double> steerings;
calcSteeringAngles(trajectory, wheelbase, steerings);

std::vector<double> steering_rates;

for (size_t i = 0; i < trajectory.points.size() - 1; ++i) {
const auto & p_prev = trajectory.points.at(i);
const auto & p_next = trajectory.points.at(i + 1);
Expand All @@ -199,7 +199,6 @@ std::vector<double> calcSteeringRates(const Trajectory & trajectory, const doubl
steering_rates.push_back(steering_rates.back());
}

return steering_rates;
}

bool checkFinite(const TrajectoryPoint & point)
Expand Down

0 comments on commit e81c91b

Please sign in to comment.