Skip to content

Commit

Permalink
refactor: split file
Browse files Browse the repository at this point in the history 
Signed-off-by: satoshi-ota <[email protected]>
  • Loading branch information
@satoshi-ota
satoshi-ota committed Sep 30, 2024
1 parent 0d59260 commit 1689a49
Show file tree
Hide file tree
Showing 8 changed files with 36 additions and 534 deletions.
1 change: 1 addition & 0 deletions planning/autoware_planning_data_analyzer/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@
<depend>autoware_perception_msgs</depend>
<depend>autoware_planning_msgs</depend>
<depend>autoware_route_handler</depend>
<depend>autoware_trajectory_evaluator</depend>
<depend>autoware_universe_utils</depend>
<depend>autoware_vehicle_info_utils</depend>
<depend>autoware_vehicle_msgs</depend>
Expand Down
58 changes: 0 additions & 58 deletions planning/autoware_planning_data_analyzer/src/data_structs.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,27 +22,6 @@

namespace autoware::behavior_analyzer
{

enum class METRIC {
LATERAL_ACCEL = 0,
LONGITUDINAL_JERK = 1,
TRAVEL_DISTANCE = 2,
MINIMUM_TTC = 3,
LATERAL_DEVIATION = 4,
TRAJECTORY_DEVIATION = 5,
SIZE
};

enum class SCORE {
LATERAL_COMFORTABILITY = 0,
LONGITUDINAL_COMFORTABILITY = 1,
EFFICIENCY = 2,
SAFETY = 3,
ACHIEVABILITY = 4,
CONSISTENCY = 5,
SIZE
};

struct TOPIC
{
static std::string TF;
Expand Down Expand Up @@ -87,18 +66,6 @@ struct EvaluatorParameters
TargetStateParameters target_state{};
};

struct SelectorParameters
{
explicit SelectorParameters(const size_t sample_num)
: time_decay_weight(static_cast<size_t>(METRIC::SIZE), std::vector<double>(sample_num, 0.0)),
score_weight(static_cast<size_t>(SCORE::SIZE), 0.0)
{
}

std::vector<std::vector<double>> time_decay_weight;
std::vector<double> score_weight;
};

struct Result
{
Result(
Expand All @@ -111,31 +78,6 @@ struct Result
std::vector<double> weight;
double loss{0.0};
};

struct CoreData
{
CoreData(
const std::shared_ptr<TrajectoryPoints> & points,
const std::shared_ptr<TrajectoryPoints> & previous_points,
const std::shared_ptr<PredictedObjects> & objects, const std::string & tag)
: points{points}, previous_points{previous_points}, objects{objects}, tag{tag}
{
}

std::shared_ptr<TrajectoryPoints> points;

std::shared_ptr<TrajectoryPoints> previous_points;

std::shared_ptr<PredictedObjects> objects;

std::string tag;
};

struct DataSet
{
std::shared_ptr<TrajectoryPoints> points;
};

} // namespace autoware::behavior_analyzer

#endif // DATA_STRUCTS_HPP_
277 changes: 5 additions & 272 deletions planning/autoware_planning_data_analyzer/src/evaluation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,275 +30,13 @@

namespace autoware::behavior_analyzer
{

DataInterface::DataInterface(
const std::shared_ptr<CoreData> & core_data, const std::shared_ptr<RouteHandler> & route_handler,
const std::shared_ptr<VehicleInfo> & vehicle_info)
: core_data_{core_data},
route_handler_{route_handler},
vehicle_info_{vehicle_info},
metrics_(static_cast<size_t>(METRIC::SIZE), std::vector<double>(core_data->points->size(), 0.0)),
scores_(static_cast<size_t>(SCORE::SIZE), 0.0)
{
evaluate();
}

void DataInterface::evaluate()
{
for (size_t i = 0; i < core_data_->points->size(); i++) {
metrics_.at(static_cast<size_t>(METRIC::LATERAL_ACCEL)).at(i) = lateral_accel(i);
metrics_.at(static_cast<size_t>(METRIC::LONGITUDINAL_JERK)).at(i) = longitudinal_jerk(i);
metrics_.at(static_cast<size_t>(METRIC::MINIMUM_TTC)).at(i) = minimum_ttc(i);
metrics_.at(static_cast<size_t>(METRIC::TRAVEL_DISTANCE)).at(i) = travel_distance(i);
metrics_.at(static_cast<size_t>(METRIC::LATERAL_DEVIATION)).at(i) = lateral_deviation(i);
metrics_.at(static_cast<size_t>(METRIC::TRAJECTORY_DEVIATION)).at(i) = trajectory_deviation(i);
}
}

void DataInterface::compress(const std::vector<std::vector<double>> & weight)
{
scores_.at(static_cast<size_t>(SCORE::LATERAL_COMFORTABILITY)) =
compress(weight, METRIC::LATERAL_ACCEL);
scores_.at(static_cast<size_t>(SCORE::LONGITUDINAL_COMFORTABILITY)) =
compress(weight, METRIC::LONGITUDINAL_JERK);
scores_.at(static_cast<size_t>(SCORE::EFFICIENCY)) = compress(weight, METRIC::TRAVEL_DISTANCE);
scores_.at(static_cast<size_t>(SCORE::SAFETY)) = compress(weight, METRIC::MINIMUM_TTC);
scores_.at(static_cast<size_t>(SCORE::ACHIEVABILITY)) =
compress(weight, METRIC::LATERAL_DEVIATION);
scores_.at(static_cast<size_t>(SCORE::CONSISTENCY)) =
compress(weight, METRIC::TRAJECTORY_DEVIATION);
}

double DataInterface::lateral_accel(const size_t idx) const
{
const auto radius =
vehicle_info_->wheel_base_m / std::tan(core_data_->points->at(idx).front_wheel_angle_rad);
const auto speed = core_data_->points->at(idx).longitudinal_velocity_mps;
return std::abs(speed * speed / radius);
}

double DataInterface::longitudinal_jerk(const size_t idx) const
{
if (idx + 2 > core_data_->points->size()) return 0.0;

const auto jerk = (core_data_->points->at(idx + 1).acceleration_mps2 -
core_data_->points->at(idx).acceleration_mps2) /
0.5;
return std::abs(jerk);
}

double DataInterface::minimum_ttc(const size_t idx) const
{
// TODO: linear interpolation
return utils::time_to_collision(core_data_->points, core_data_->objects, idx);
}

double DataInterface::travel_distance(const size_t idx) const
{
return autoware::motion_utils::calcSignedArcLength(*core_data_->points, 0L, idx);
}

double DataInterface::lateral_deviation(const size_t idx) const
{
lanelet::ConstLanelet nearest{};
if (!route_handler_->getClosestPreferredLaneletWithinRoute(
autoware::universe_utils::getPose(core_data_->points->at(idx)), &nearest)) {
return std::numeric_limits<double>::max();
}
const auto arc_coordinates = lanelet::utils::getArcCoordinates(
{nearest}, autoware::universe_utils::getPose(core_data_->points->at(idx)));
return std::abs(arc_coordinates.distance);
}

double DataInterface::trajectory_deviation(const size_t idx) const
{
if (core_data_->previous_points == nullptr) return 0.0;

if (idx + 1 > core_data_->previous_points->size()) return 0.0;

const auto & p1 = autoware::universe_utils::getPose(core_data_->points->at(idx));
const auto & p2 = autoware::universe_utils::getPose(core_data_->previous_points->at(idx));
return autoware::universe_utils::calcSquaredDistance2d(p1, p2);
}

bool DataInterface::feasible() const
{
const auto condition = [](const auto & p) { return p.longitudinal_velocity_mps >= 0.0; };
return std::all_of(core_data_->points->begin(), core_data_->points->end(), condition);
}

void DataInterface::normalize(
const double min, const double max, const size_t score_type, const bool flip)
{
scores_.at(score_type) = flip ? (max - scores_.at(score_type)) / (max - min)
: (scores_.at(score_type) - min) / (max - min);
}

auto DataInterface::compress(
const std::vector<std::vector<double>> & weight, const METRIC & metric_type) const -> double
{
const auto w = weight.at(static_cast<size_t>(metric_type));
const auto metric = metrics_.at(static_cast<size_t>(metric_type));
return std::inner_product(w.begin(), w.end(), metric.begin(), 0.0);
}

auto DataInterface::score(const SCORE & score_type) const -> double
{
return scores_.at(static_cast<size_t>(score_type));
}

void DataInterface::weighting(const std::vector<double> & weight)
{
total_ = std::inner_product(weight.begin(), weight.end(), scores_.begin(), 0.0);
}

void Evaluator::normalize()
{
const auto range = [this](const size_t idx) {
const auto min_itr = std::min_element(
results_.begin(), results_.end(),
[&idx](const auto & a, const auto & b) { return a->scores().at(idx) < b->scores().at(idx); });
const auto max_itr = std::max_element(
results_.begin(), results_.end(),
[&idx](const auto & a, const auto & b) { return a->scores().at(idx) < b->scores().at(idx); });

return std::make_pair((*min_itr)->scores().at(idx), (*max_itr)->scores().at(idx));
};

const auto [s0_min, s0_max] = range(static_cast<size_t>(SCORE::LATERAL_COMFORTABILITY));
const auto [s1_min, s1_max] = range(static_cast<size_t>(SCORE::LONGITUDINAL_COMFORTABILITY));
const auto [s2_min, s2_max] = range(static_cast<size_t>(SCORE::EFFICIENCY));
const auto [s3_min, s3_max] = range(static_cast<size_t>(SCORE::SAFETY));
const auto [s4_min, s4_max] = range(static_cast<size_t>(SCORE::ACHIEVABILITY));
const auto [s5_min, s5_max] = range(static_cast<size_t>(SCORE::CONSISTENCY));

for (auto & data : results_) {
data->normalize(s0_min, s0_max, static_cast<size_t>(SCORE::LATERAL_COMFORTABILITY), true);
data->normalize(s1_min, s1_max, static_cast<size_t>(SCORE::LONGITUDINAL_COMFORTABILITY), true);
data->normalize(s2_min, s2_max, static_cast<size_t>(SCORE::EFFICIENCY));
data->normalize(s3_min, s3_max, static_cast<size_t>(SCORE::SAFETY));
data->normalize(s4_min, s4_max, static_cast<size_t>(SCORE::ACHIEVABILITY), true);
data->normalize(s5_min, s5_max, static_cast<size_t>(SCORE::CONSISTENCY), true);
}
}

void Evaluator::pruning()
{
const auto itr =
std::remove_if(results_.begin(), results_.end(), [](const auto & d) { return !d->feasible(); });

results_.erase(itr, results_.end());
}

void Evaluator::compress(const std::vector<std::vector<double>> & weight)
{
std::for_each(
results_.begin(), results_.end(), [&weight](auto & data) { data->compress(weight); });
}

void Evaluator::weighting(const std::vector<double> & weight)
{
std::for_each(
results_.begin(), results_.end(), [&weight](auto & data) { data->weighting(weight); });

std::sort(results_.begin(), results_.end(), [](const auto & a, const auto & b) {
return a->total() > b->total();
});
}

auto Evaluator::get(const std::string & tag) const -> std::shared_ptr<DataInterface>
{
const auto itr = std::find_if(
results_.begin(), results_.end(), [&tag](const auto & data) { return data->tag() == tag; });

return itr != results_.end() ? *itr : nullptr;
}

void Evaluator::add(const std::shared_ptr<CoreData> & core_data)
{
const auto ptr = std::make_shared<DataInterface>(core_data, route_handler_, vehicle_info_);
results_.push_back(ptr);
}

auto Evaluator::best(const std::shared_ptr<SelectorParameters> & parameters)
-> std::shared_ptr<DataInterface>
{
pruning();

compress(parameters->time_decay_weight);

normalize();

weighting(parameters->score_weight);

return best();
}

auto Evaluator::best() const -> std::shared_ptr<DataInterface>
{
if (results_.empty()) return nullptr;

if (!results_.front()->feasible()) return nullptr;

return results_.front();
}

auto Evaluator::statistics(const SCORE & score_type) const -> std::pair<double, double>
{
double ave = 0.0;
double dev = 0.0;

const auto update = [](const double ave, const double dev, const double value, const size_t i) {
const auto new_ave = (i * ave + value) / (i + 1);
const auto new_dev =
(i * (ave * ave + dev * dev) + value * value) / (i + 1) - new_ave * new_ave;
return std::make_pair(new_ave, new_dev);
};

for (size_t i = 0; i < results_.size(); i++) {
std::tie(ave, dev) = update(ave, dev, results_.at(i)->score(score_type), i);
}

return std::make_pair(ave, dev);
}

void Evaluator::show() const
{
const auto best_data = best();

if (best_data == nullptr) {
return;
}

const auto s0 = statistics(SCORE::LATERAL_COMFORTABILITY);
const auto s1 = statistics(SCORE::LONGITUDINAL_COMFORTABILITY);
const auto s2 = statistics(SCORE::EFFICIENCY);
const auto s3 = statistics(SCORE::SAFETY);
const auto s4 = statistics(SCORE::ACHIEVABILITY);
const auto s5 = statistics(SCORE::CONSISTENCY);

std::stringstream ss;
ss << std::fixed << std::setprecision(2) << "\n";
// clang-format off
ss << " tag :" << best_data->tag() << "\n";
ss << " lat comfortability:" << best_data->score(SCORE::LATERAL_COMFORTABILITY) << " mean:" << s0.first << " std:" << std::sqrt(s0.second) << "\n"; // NOLINT
ss << " lon comfortability:" << best_data->score(SCORE::LONGITUDINAL_COMFORTABILITY) << " mean:" << s1.first << " std:" << std::sqrt(s1.second) << "\n"; // NOLINT
ss << " efficiency :" << best_data->score(SCORE::EFFICIENCY) << " mean:" << s2.first << " std:" << std::sqrt(s2.second) << "\n"; // NOLINT
ss << " safety :" << best_data->score(SCORE::SAFETY) << " mean:" << s3.first << " std:" << std::sqrt(s3.second) << "\n"; // NOLINT
ss << " achievability :" << best_data->score(SCORE::ACHIEVABILITY) << " mean:" << s4.first << " std:" << std::sqrt(s4.second) << "\n"; // NOLINT
ss << " consistency :" << best_data->score(SCORE::CONSISTENCY) << " mean:" << s5.first << " std:" << std::sqrt(s5.second) << "\n"; // NOLINT
ss << " total :" << best_data->total();
// clang-format on
RCLCPP_INFO_STREAM(rclcpp::get_logger(__func__), ss.str());
}

BagEvaluator::BagEvaluator(
const std::shared_ptr<BagData> & bag_data,
const std::shared_ptr<TrajectoryPoints> & previous_points,
const std::shared_ptr<RouteHandler> & route_handler,
const std::shared_ptr<VehicleInfo> & vehicle_info,
const std::shared_ptr<EvaluatorParameters> & evaluator_parameters)
: Evaluator{route_handler, vehicle_info}
: autoware::trajectory_selector::trajectory_evaluator::Evaluator{route_handler, vehicle_info}
{
const auto odometry_buffer_ptr =
std::dynamic_pointer_cast<Buffer<Odometry>>(bag_data->buffers.at(TOPIC::ODOMETRY));
Expand Down Expand Up @@ -377,7 +115,7 @@ BagEvaluator::BagEvaluator(
}

{
const auto core_data = std::make_shared<CoreData>(
const auto core_data = std::make_shared<trajectory_selector::trajectory_evaluator::CoreData>(
std::make_shared<TrajectoryPoints>(points), previous_points, current_objects,
"ground_truth");

Expand Down Expand Up @@ -411,23 +149,18 @@ BagEvaluator::BagEvaluator(
for (const auto & points : utils::sampling(
*trajectory_ptr, odometry_ptr->pose.pose, odometry_ptr->twist.twist.linear.x,
accel_ptr->accel.accel.linear.x, vehicle_info, evaluator_parameters)) {
const auto core_data = std::make_shared<CoreData>(
const auto core_data = std::make_shared<trajectory_selector::trajectory_evaluator::CoreData>(
std::make_shared<TrajectoryPoints>(points), previous_points, current_objects, "candidates");

add(core_data);
}
}
}

auto BagEvaluator::loss(const std::shared_ptr<SelectorParameters> & parameters)
auto BagEvaluator::loss(
const std::shared_ptr<trajectory_selector::trajectory_evaluator::SelectorParameters> & parameters)
-> std::pair<double, std::shared_ptr<TrajectoryPoints>>
{
// pruning();

// normalize();

// weighting(weight);

const auto best_data = best(parameters);

if (best_data == nullptr) {
Expand Down
Loading

0 comments on commit 1689a49

Please sign in to comment.