diff --git a/planning/autoware_planning_data_analyzer/src/data_structs.hpp b/planning/autoware_planning_data_analyzer/src/data_structs.hpp index 260a455c..e0450135 100644 --- a/planning/autoware_planning_data_analyzer/src/data_structs.hpp +++ b/planning/autoware_planning_data_analyzer/src/data_structs.hpp @@ -25,12 +25,11 @@ namespace autoware::behavior_analyzer enum class METRIC { LATERAL_ACCEL = 0, - LONGITUDINAL_ACCEL = 1, - LONGITUDINAL_JERK = 2, - TRAVEL_DISTANCE = 3, - MINIMUM_TTC = 4, - LATERAL_DEVIATION = 5, - TRAJECTORY_DEVIATION = 6, + LONGITUDINAL_JERK = 1, + TRAVEL_DISTANCE = 2, + MINIMUM_TTC = 3, + LATERAL_DEVIATION = 4, + TRAJECTORY_DEVIATION = 5, SIZE }; @@ -41,7 +40,6 @@ enum class SCORE { SAFETY = 3, ACHIEVABILITY = 4, CONSISTENCY = 5, - TOTAL = 6, SIZE }; diff --git a/planning/autoware_planning_data_analyzer/src/evaluation.cpp b/planning/autoware_planning_data_analyzer/src/evaluation.cpp index 2e086cd5..ad098ff2 100644 --- a/planning/autoware_planning_data_analyzer/src/evaluation.cpp +++ b/planning/autoware_planning_data_analyzer/src/evaluation.cpp @@ -33,94 +33,62 @@ namespace autoware::behavior_analyzer DataInterface::DataInterface( const std::shared_ptr & bag_data, const std::shared_ptr & previous, - const vehicle_info_utils::VehicleInfo & vehicle_info, - const std::shared_ptr & parameters, const std::string & tag, - const std::shared_ptr & points) + const std::shared_ptr & vehicle_info, const std::shared_ptr & parameters, + const std::string & tag, const std::shared_ptr & points) : previous_{previous}, - vehicle_info{vehicle_info}, - route_handler{bag_data->route_handler}, - parameters{parameters}, - tag{tag}, - points_{points} + vehicle_info_{vehicle_info}, + route_handler_{bag_data->route_handler}, + parameters_{parameters}, + tag_{tag}, + points_{points}, + values_{static_cast(METRIC::SIZE), std::vector(parameters->resample_num, 0.0)}, + scores_{static_cast(SCORE::SIZE)} { - objects_history.reserve(parameters->resample_num); + objects_history.reserve(parameters_->resample_num); const auto objects_buffer_ptr = std::dynamic_pointer_cast>(bag_data->buffers.at(TOPIC::OBJECTS)); - for (size_t i = 0; i < parameters->resample_num; i++) { + for (size_t i = 0; i < parameters_->resample_num; i++) { const auto opt_objects = - objects_buffer_ptr->get(bag_data->timestamp + 1e9 * parameters->time_resolution * i); + objects_buffer_ptr->get(bag_data->timestamp + 1e9 * parameters_->time_resolution * i); if (!opt_objects) { break; } objects_history.push_back(opt_objects); } - - values.resize(static_cast(METRIC::SIZE)); - scores.resize(static_cast(SCORE::SIZE)); } -void DataInterface::calculate() +void DataInterface::evaluate() { - if (!feasible()) { - scores.at(static_cast(SCORE::LATERAL_COMFORTABILITY)) = 0.0; - scores.at(static_cast(SCORE::LONGITUDINAL_COMFORTABILITY)) = 0.0; - scores.at(static_cast(SCORE::EFFICIENCY)) = 0.0; - scores.at(static_cast(SCORE::SAFETY)) = 0.0; - scores.at(static_cast(SCORE::ACHIEVABILITY)) = 0.0; - scores.at(static_cast(SCORE::CONSISTENCY)) = 0.0; - } - - std::vector lateral_accel_values; - std::vector minimum_ttc_values; - std::vector longitudinal_jerk_values; - std::vector travel_distance_values; - std::vector lateral_deviation_values; - std::vector trajectory_deviation_values; - - for (size_t i = 0; i < parameters->resample_num - 1; i++) { - lateral_accel_values.push_back(lateral_accel(i)); - longitudinal_jerk_values.push_back(longitudinal_jerk(i)); - minimum_ttc_values.push_back(minimum_ttc(i)); - travel_distance_values.push_back(travel_distance(i)); - lateral_deviation_values.push_back(lateral_deviation(i)); - trajectory_deviation_values.push_back(trajectory_deviation(i)); - } - - { - lateral_accel_values.push_back(lateral_accel(parameters->resample_num - 1)); - longitudinal_jerk_values.push_back(0.0); - minimum_ttc_values.push_back(minimum_ttc(parameters->resample_num - 1)); - travel_distance_values.push_back(travel_distance(parameters->resample_num - 1)); - lateral_deviation_values.push_back(lateral_deviation(parameters->resample_num - 1)); - trajectory_deviation_values.push_back(trajectory_deviation(parameters->resample_num - 1)); + for (size_t i = 0; i < parameters_->resample_num; i++) { + values_.at(static_cast(METRIC::LATERAL_ACCEL)).at(i) = lateral_accel(i); + values_.at(static_cast(METRIC::LONGITUDINAL_JERK)).at(i) = longitudinal_jerk(i); + values_.at(static_cast(METRIC::MINIMUM_TTC)).at(i) = minimum_ttc(i); + values_.at(static_cast(METRIC::TRAVEL_DISTANCE)).at(i) = travel_distance(i); + values_.at(static_cast(METRIC::LATERAL_DEVIATION)).at(i) = lateral_deviation(i); + values_.at(static_cast(METRIC::TRAJECTORY_DEVIATION)).at(i) = trajectory_deviation(i); } - values.at(static_cast(METRIC::LATERAL_ACCEL)) = lateral_accel_values; - values.at(static_cast(METRIC::LONGITUDINAL_JERK)) = longitudinal_jerk_values; - values.at(static_cast(METRIC::MINIMUM_TTC)) = minimum_ttc_values; - values.at(static_cast(METRIC::TRAVEL_DISTANCE)) = travel_distance_values; - values.at(static_cast(METRIC::LATERAL_DEVIATION)) = lateral_deviation_values; - values.at(static_cast(METRIC::TRAJECTORY_DEVIATION)) = trajectory_deviation_values; - - scores.at(static_cast(SCORE::LATERAL_COMFORTABILITY)) = to_1d(METRIC::LATERAL_ACCEL); - scores.at(static_cast(SCORE::LONGITUDINAL_COMFORTABILITY)) = - to_1d(METRIC::LONGITUDINAL_JERK); - scores.at(static_cast(SCORE::EFFICIENCY)) = to_1d(METRIC::TRAVEL_DISTANCE); - scores.at(static_cast(SCORE::SAFETY)) = to_1d(METRIC::MINIMUM_TTC); - scores.at(static_cast(SCORE::ACHIEVABILITY)) = to_1d(METRIC::LATERAL_DEVIATION); - scores.at(static_cast(SCORE::CONSISTENCY)) = to_1d(METRIC::TRAJECTORY_DEVIATION); + scores_.at(static_cast(SCORE::LATERAL_COMFORTABILITY)) = compress(METRIC::LATERAL_ACCEL); + scores_.at(static_cast(SCORE::LONGITUDINAL_COMFORTABILITY)) = + compress(METRIC::LONGITUDINAL_JERK); + scores_.at(static_cast(SCORE::EFFICIENCY)) = compress(METRIC::TRAVEL_DISTANCE); + scores_.at(static_cast(SCORE::SAFETY)) = compress(METRIC::MINIMUM_TTC); + scores_.at(static_cast(SCORE::ACHIEVABILITY)) = compress(METRIC::LATERAL_DEVIATION); + scores_.at(static_cast(SCORE::CONSISTENCY)) = compress(METRIC::TRAJECTORY_DEVIATION); } double DataInterface::lateral_accel(const size_t idx) const { - const auto radius = vehicle_info.wheel_base_m / std::tan(points_->at(idx).front_wheel_angle_rad); + const auto radius = + vehicle_info_->wheel_base_m / std::tan(points_->at(idx).front_wheel_angle_rad); const auto speed = points_->at(idx).longitudinal_velocity_mps; return speed * speed / radius; } double DataInterface::longitudinal_jerk(const size_t idx) const { + if (idx + 1 == points_->size()) return 0.0; return (points_->at(idx + 1).acceleration_mps2 - points_->at(idx).acceleration_mps2) / 0.5; } @@ -140,7 +108,7 @@ double DataInterface::travel_distance(const size_t idx) const double DataInterface::lateral_deviation(const size_t idx) const { lanelet::ConstLanelet nearest{}; - if (!route_handler->getClosestPreferredLaneletWithinRoute( + if (!route_handler_->getClosestPreferredLaneletWithinRoute( autoware::universe_utils::getPose(points_->at(idx)), &nearest)) { return std::numeric_limits::max(); } @@ -166,11 +134,11 @@ bool DataInterface::feasible() const bool DataInterface::ready() const { - if (objects_history.size() < parameters->resample_num) { + if (objects_history.size() < parameters_->resample_num) { return false; } - if (points_->size() < parameters->resample_num) { + if (points_->size() < parameters_->resample_num) { return false; } @@ -180,42 +148,43 @@ bool DataInterface::ready() const 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); + scores_.at(score_type) = flip ? (max - scores_.at(score_type)) / (max - min) + : (scores_.at(score_type) - min) / (max - min); } -auto DataInterface::to_1d(const METRIC & metric_type) const -> double +auto DataInterface::compress(const METRIC & metric_type) const -> double { constexpr double TIME_FACTOR = 0.8; double score = 0.0; - for (size_t i = 0; i < parameters->resample_num; i++) { - score += std::pow(TIME_FACTOR, i) * std::abs(values.at(static_cast(metric_type)).at(i)); + for (size_t i = 0; i < parameters_->resample_num; i++) { + score += + std::pow(TIME_FACTOR, i) * std::abs(values_.at(static_cast(metric_type)).at(i)); } return score; } -double DataInterface::get(const SCORE & score_type) const +auto DataInterface::score(const SCORE & score_type) const -> double { - return scores.at(static_cast(score_type)); + return scores_.at(static_cast(score_type)); } -auto DataInterface::total(const std::vector & weight) const -> double +void DataInterface::weighting(const std::vector & weight) { - return std::inner_product(weight.begin(), weight.end(), scores.begin(), 0.0); + total_ = std::inner_product(weight.begin(), weight.end(), scores_.begin(), 0.0); } GroundTruth::GroundTruth( const std::shared_ptr & bag_data, const std::shared_ptr & prev_best_data, - const vehicle_info_utils::VehicleInfo & vehicle_info, - const std::shared_ptr & parameters, const std::string & tag) + const std::shared_ptr & vehicle_info, const std::shared_ptr & parameters, + const std::string & tag) : DataInterface( bag_data, prev_best_data, vehicle_info, parameters, tag, to_points(bag_data, parameters)) { - calculate(); + evaluate(); } auto GroundTruth::to_points( @@ -270,20 +239,18 @@ auto GroundTruth::to_points( TrajectoryData::TrajectoryData( const std::shared_ptr & bag_data, const std::shared_ptr & prev_best_data, - const vehicle_info_utils::VehicleInfo & vehicle_info, - const std::shared_ptr & parameters, const std::string & tag, - const std::shared_ptr & points) + const std::shared_ptr & vehicle_info, const std::shared_ptr & parameters, + const std::string & tag, const std::shared_ptr & points) : DataInterface(bag_data, prev_best_data, vehicle_info, parameters, tag, points) { - calculate(); + evaluate(); } Evaluator::Evaluator( const std::shared_ptr & bag_data, const std::shared_ptr & prev_best_data, - const vehicle_info_utils::VehicleInfo & vehicle_info, - const std::shared_ptr & parameters) -: parameters{parameters} + const std::shared_ptr & vehicle_info, const std::shared_ptr & parameters) +: results_{}, parameters_{parameters} { const auto opt_odometry = std::dynamic_pointer_cast>( bag_data->buffers.at("/localization/kinematic_state")) @@ -309,34 +276,34 @@ Evaluator::Evaluator( // frenet planner for (const auto & points : utils::sampling( *opt_trajectory, opt_odometry->pose.pose, opt_odometry->twist.twist.linear.x, - opt_accel->accel.accel.linear.x, vehicle_info, parameters)) { + opt_accel->accel.accel.linear.x, vehicle_info, parameters_)) { const auto ptr = std::make_shared( - bag_data, prev_best_data, vehicle_info, parameters, "frenet", + bag_data, prev_best_data, vehicle_info, parameters_, "frenet", std::make_shared(points)); - data_set.push_back(static_cast>(ptr)); + results_.push_back(static_cast>(ptr)); } // actual behavior { const auto ptr = std::make_shared( - bag_data, prev_best_data, vehicle_info, parameters, "ground_truth"); - data_set.push_back(static_cast>(ptr)); + bag_data, prev_best_data, vehicle_info, parameters_, "ground_truth"); + results_.push_back(static_cast>(ptr)); } - normalize_scores(); + normalize(); } -void Evaluator::normalize_scores() +void Evaluator::normalize() { const auto range = [this](const size_t idx) { const auto min_itr = std::min_element( - data_set.begin(), data_set.end(), - [&idx](const auto & a, const auto & b) { return a->scores.at(idx) < b->scores.at(idx); }); + 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( - data_set.begin(), data_set.end(), - [&idx](const auto & a, const auto & b) { return a->scores.at(idx) < b->scores.at(idx); }); + 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)); + return std::make_pair((*min_itr)->scores().at(idx), (*max_itr)->scores().at(idx)); }; const auto [s0_min, s0_max] = range(static_cast(SCORE::LATERAL_COMFORTABILITY)); @@ -346,52 +313,60 @@ void Evaluator::normalize_scores() const auto [s4_min, s4_max] = range(static_cast(SCORE::ACHIEVABILITY)); const auto [s5_min, s5_max] = range(static_cast(SCORE::CONSISTENCY)); - for (auto & data : data_set) { + for (auto & data : results_) { data->normalize(s0_min, s0_max, static_cast(SCORE::LATERAL_COMFORTABILITY), true); data->normalize(s1_min, s1_max, static_cast(SCORE::LONGITUDINAL_COMFORTABILITY), true); data->normalize(s2_min, s2_max, static_cast(SCORE::EFFICIENCY)); data->normalize(s3_min, s3_max, static_cast(SCORE::SAFETY)); data->normalize(s4_min, s4_max, static_cast(SCORE::ACHIEVABILITY), true); data->normalize(s5_min, s5_max, static_cast(SCORE::CONSISTENCY), true); - data->scores.at(static_cast(SCORE::TOTAL)) = data->total(parameters->weight); } +} - const auto [total_min, total_max] = range(static_cast(SCORE::TOTAL)); +void Evaluator::weighting(const std::vector & weight) +{ + std::for_each( + results_.begin(), results_.end(), [&weight](auto & data) { data->weighting(weight); }); - for (auto & data : data_set) { - data->normalize(total_min, total_max, static_cast(SCORE::TOTAL)); - } + std::sort(results_.begin(), results_.end(), [](const auto & a, const auto & b) { + return a->total() > b->total(); + }); + + const auto itr = + std::remove_if(results_.begin(), results_.end(), [](const auto & d) { return !d->feasible(); }); + + results_.erase(itr, results_.end()); } auto Evaluator::get(const std::string & tag) const -> std::shared_ptr { const auto itr = std::find_if( - data_set.begin(), data_set.end(), [&tag](const auto & data) { return data->tag == tag; }); + results_.begin(), results_.end(), [&tag](const auto & data) { return data->tag() == tag; }); - return itr != data_set.end() ? *itr : nullptr; + return itr != results_.end() ? *itr : nullptr; } -auto Evaluator::best(const std::vector & weight) const -> std::shared_ptr +auto Evaluator::best(const std::vector & weight) -> std::shared_ptr { - auto sort_by_score = data_set; - - std::sort(sort_by_score.begin(), sort_by_score.end(), [&weight](const auto & a, const auto & b) { - return a->total(weight) > b->total(weight); - }); + weighting(weight); - const auto itr = std::remove_if( - sort_by_score.begin(), sort_by_score.end(), [](const auto & d) { return !d->feasible(); }); + return best(); +} - sort_by_score.erase(itr, sort_by_score.end()); +auto Evaluator::best() const -> std::shared_ptr +{ + if (results_.empty()) return nullptr; - if (sort_by_score.empty()) return nullptr; + if (!results_.front()->feasible()) return nullptr; - return sort_by_score.front(); + return results_.front(); } -auto Evaluator::loss(const std::vector & weight) const -> double +auto Evaluator::loss(const std::vector & weight) -> double { - const auto best_data = best(weight); + weighting(weight); + + const auto best_data = best(); if (best_data == nullptr) { return 0.0; } @@ -413,9 +388,9 @@ auto Evaluator::loss(const std::vector & weight) const -> double return mse; } -void Evaluator::show() +void Evaluator::show() const { - const auto best_data = best(parameters->weight); + const auto best_data = best(); if (best_data == nullptr) { return; @@ -441,39 +416,30 @@ void Evaluator::show() return std::make_pair(new_ave, new_dev); }; - for (size_t i = 0; i < data_set.size(); i++) { - const auto data = data_set.at(i); - std::tie(s0_ave, s0_dev) = update(s0_ave, s0_dev, data->get(SCORE::LATERAL_COMFORTABILITY), i); + for (size_t i = 0; i < results_.size(); i++) { + const auto data = results_.at(i); + std::tie(s0_ave, s0_dev) = + update(s0_ave, s0_dev, data->score(SCORE::LATERAL_COMFORTABILITY), i); std::tie(s1_ave, s1_dev) = - update(s1_ave, s1_dev, data->get(SCORE::LONGITUDINAL_COMFORTABILITY), i); - std::tie(s2_ave, s2_dev) = update(s2_ave, s2_dev, data->get(SCORE::EFFICIENCY), i); - std::tie(s3_ave, s3_dev) = update(s3_ave, s3_dev, data->get(SCORE::SAFETY), i); - std::tie(s4_ave, s4_dev) = update(s4_ave, s4_dev, data->get(SCORE::ACHIEVABILITY), i); - std::tie(s5_ave, s5_dev) = update(s5_ave, s5_dev, data->get(SCORE::CONSISTENCY), i); + update(s1_ave, s1_dev, data->score(SCORE::LONGITUDINAL_COMFORTABILITY), i); + std::tie(s2_ave, s2_dev) = update(s2_ave, s2_dev, data->score(SCORE::EFFICIENCY), i); + std::tie(s3_ave, s3_dev) = update(s3_ave, s3_dev, data->score(SCORE::SAFETY), i); + std::tie(s4_ave, s4_dev) = update(s4_ave, s4_dev, data->score(SCORE::ACHIEVABILITY), i); + std::tie(s5_ave, s5_dev) = update(s5_ave, s5_dev, data->score(SCORE::CONSISTENCY), i); } std::stringstream ss; ss << std::fixed << std::setprecision(2) << "\n"; // clang-format off - ss << " tag :" << best_data->tag << "\n"; - ss << " lat comfortability:" << best_data->get(SCORE::LATERAL_COMFORTABILITY) << " mean:" << s0_ave << " std:" << std::sqrt(s0_dev) << "\n"; // NOLINT - ss << " lon comfortability:" << best_data->get(SCORE::LONGITUDINAL_COMFORTABILITY) << " mean:" << s1_ave << " std:" << std::sqrt(s1_dev) << "\n"; // NOLINT - ss << " efficiency :" << best_data->get(SCORE::EFFICIENCY) << " mean:" << s2_ave << " std:" << std::sqrt(s2_dev) << "\n"; // NOLINT - ss << " safety :" << best_data->get(SCORE::SAFETY) << " mean:" << s3_ave << " std:" << std::sqrt(s3_dev) << "\n"; // NOLINT - ss << " achievability :" << best_data->get(SCORE::ACHIEVABILITY) << " mean:" << s4_ave << " std:" << std::sqrt(s4_dev) << "\n"; // NOLINT - ss << " consistency :" << best_data->get(SCORE::CONSISTENCY) << " mean:" << s5_ave << " std:" << std::sqrt(s5_dev) << "\n"; // NOLINT - ss << " total :" << best_data->get(SCORE::TOTAL); + ss << " tag :" << best_data->tag() << "\n"; + ss << " lat comfortability:" << best_data->score(SCORE::LATERAL_COMFORTABILITY) << " mean:" << s0_ave << " std:" << std::sqrt(s0_dev) << "\n"; // NOLINT + ss << " lon comfortability:" << best_data->score(SCORE::LONGITUDINAL_COMFORTABILITY) << " mean:" << s1_ave << " std:" << std::sqrt(s1_dev) << "\n"; // NOLINT + ss << " efficiency :" << best_data->score(SCORE::EFFICIENCY) << " mean:" << s2_ave << " std:" << std::sqrt(s2_dev) << "\n"; // NOLINT + ss << " safety :" << best_data->score(SCORE::SAFETY) << " mean:" << s3_ave << " std:" << std::sqrt(s3_dev) << "\n"; // NOLINT + ss << " achievability :" << best_data->score(SCORE::ACHIEVABILITY) << " mean:" << s4_ave << " std:" << std::sqrt(s4_dev) << "\n"; // NOLINT + ss << " consistency :" << best_data->score(SCORE::CONSISTENCY) << " mean:" << s5_ave << " std:" << std::sqrt(s5_dev) << "\n"; // NOLINT + ss << " total :" << best_data->total(); // clang-format on RCLCPP_INFO_STREAM(rclcpp::get_logger(__func__), ss.str()); } - -auto Evaluator::get(const SCORE & score_type) const -> std::vector -{ - std::vector ret; - for (const auto & data : data_set) { - const auto value = data->get(score_type); - if (std::isfinite(value)) ret.push_back(value); - } - return ret; -} } // namespace autoware::behavior_analyzer diff --git a/planning/autoware_planning_data_analyzer/src/evaluation.hpp b/planning/autoware_planning_data_analyzer/src/evaluation.hpp index fb3ef92d..6243b25e 100644 --- a/planning/autoware_planning_data_analyzer/src/evaluation.hpp +++ b/planning/autoware_planning_data_analyzer/src/evaluation.hpp @@ -32,58 +32,66 @@ class DataInterface public: DataInterface( const std::shared_ptr & bag_data, const std::shared_ptr & previous, - const vehicle_info_utils::VehicleInfo & vehicle_info, + const std::shared_ptr & vehicle_info, const std::shared_ptr & parameters, const std::string & tag, const std::shared_ptr & points); - void calculate(); - void normalize( const double min, const double max, const size_t score_type, const bool flip = false); - auto to_1d(const METRIC & metric_type) const -> double; + void weighting(const std::vector & weight); - auto total(const std::vector & weight) const -> double; + auto total() const -> double { return total_; }; - double get(const SCORE & score_type) const; + bool feasible() const; - auto score() const -> std::vector { return scores; } + bool ready() const; - double lateral_accel(const size_t idx) const; + auto score(const SCORE & score_type) const -> double; - double longitudinal_jerk(const size_t idx) const; + auto scores() const -> std::vector { return scores_; } - double minimum_ttc(const size_t idx) const; + auto points() const -> std::shared_ptr { return points_; } - double travel_distance(const size_t idx) const; + auto tag() const -> std::string { return tag_; } - double lateral_deviation(const size_t idx) const; +protected: + void evaluate(); - double trajectory_deviation(const size_t idx) const; +private: + auto lateral_accel(const size_t idx) const -> double; - bool feasible() const; + auto longitudinal_jerk(const size_t idx) const -> double; - bool ready() const; + auto minimum_ttc(const size_t idx) const -> double; - auto points() const -> std::shared_ptr { return points_; } + auto travel_distance(const size_t idx) const -> double; - std::shared_ptr previous_; + auto lateral_deviation(const size_t idx) const -> double; - std::vector objects_history; + auto trajectory_deviation(const size_t idx) const -> double; - std::vector> values; + auto compress(const METRIC & metric_type) const -> double; - std::vector scores; + std::vector objects_history; - vehicle_info_utils::VehicleInfo vehicle_info; + std::vector> values_; - std::shared_ptr route_handler; + std::vector scores_; - std::shared_ptr parameters; + std::shared_ptr previous_; std::shared_ptr points_; - std::string tag{""}; + std::shared_ptr vehicle_info_; + + std::shared_ptr route_handler_; + + std::shared_ptr parameters_; + + std::string tag_; + + double total_; }; class GroundTruth : public DataInterface @@ -92,7 +100,7 @@ class GroundTruth : public DataInterface GroundTruth( const std::shared_ptr & bag_data, const std::shared_ptr & prev_best_data, - const vehicle_info_utils::VehicleInfo & vehicle_info, + const std::shared_ptr & vehicle_info, const std::shared_ptr & parameters, const std::string & tag); private: @@ -107,7 +115,7 @@ class TrajectoryData : public DataInterface TrajectoryData( const std::shared_ptr & bag_data, const std::shared_ptr & prev_best_data, - const vehicle_info_utils::VehicleInfo & vehicle_info, + const std::shared_ptr & vehicle_info, const std::shared_ptr & parameters, const std::string & tag, const std::shared_ptr & points); }; @@ -118,26 +126,29 @@ class Evaluator Evaluator( const std::shared_ptr & bag_data, const std::shared_ptr & prev_best_data, - const vehicle_info_utils::VehicleInfo & vehicle_info, + const std::shared_ptr & vehicle_info, const std::shared_ptr & parameters); - void show(); - - void normalize_scores(); + void show() const; - auto loss(const std::vector & weight) const -> double; + auto results() const -> std::vector> { return results_; } - auto get(const SCORE & score_type) const -> std::vector; + auto best(const std::vector & weight) -> std::shared_ptr; - auto best(const std::vector & weight) const -> std::shared_ptr; + auto loss(const std::vector & weight) -> double; auto get(const std::string & tag) const -> std::shared_ptr; - std::vector> data_set; +private: + void normalize(); + + void weighting(const std::vector & weight); + + auto best() const -> std::shared_ptr; - std::shared_ptr route_handler; + std::vector> results_; - std::shared_ptr parameters; + std::shared_ptr parameters_; }; } // namespace autoware::behavior_analyzer diff --git a/planning/autoware_planning_data_analyzer/src/node.cpp b/planning/autoware_planning_data_analyzer/src/node.cpp index bcced252..5a822398 100644 --- a/planning/autoware_planning_data_analyzer/src/node.cpp +++ b/planning/autoware_planning_data_analyzer/src/node.cpp @@ -40,7 +40,8 @@ BehaviorAnalyzerNode::BehaviorAnalyzerNode(const rclcpp::NodeOptions & node_opti timer_->cancel(); - vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); + vehicle_info_ = std::make_shared( + autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()); pub_marker_ = create_publisher("~/marker", 1); pub_odometry_ = create_publisher(TOPIC::ODOMETRY, rclcpp::QoS(1)); @@ -526,47 +527,46 @@ void BehaviorAnalyzerNode::visualize(const std::shared_ptr & data_set previous_ = nullptr; } - for (size_t i = 0; i < data_set->data_set.size(); ++i) { - const auto data = data_set->data_set.at(i); - msg.markers.push_back(utils::to_marker(data, SCORE::LATERAL_COMFORTABILITY, i)); - msg.markers.push_back(utils::to_marker(data, SCORE::LONGITUDINAL_COMFORTABILITY, i)); - msg.markers.push_back(utils::to_marker(data, SCORE::EFFICIENCY, i)); - msg.markers.push_back(utils::to_marker(data, SCORE::SAFETY, i)); - msg.markers.push_back(utils::to_marker(data, SCORE::ACHIEVABILITY, i)); - msg.markers.push_back(utils::to_marker(data, SCORE::CONSISTENCY, i)); - msg.markers.push_back(utils::to_marker(data, SCORE::TOTAL, i)); + const auto results = data_set->results(); + for (size_t i = 0; i < results.size(); ++i) { + msg.markers.push_back(utils::to_marker(results.at(i), SCORE::LATERAL_COMFORTABILITY, i)); + msg.markers.push_back(utils::to_marker(results.at(i), SCORE::LONGITUDINAL_COMFORTABILITY, i)); + msg.markers.push_back(utils::to_marker(results.at(i), SCORE::EFFICIENCY, i)); + msg.markers.push_back(utils::to_marker(results.at(i), SCORE::SAFETY, i)); + msg.markers.push_back(utils::to_marker(results.at(i), SCORE::ACHIEVABILITY, i)); + msg.markers.push_back(utils::to_marker(results.at(i), SCORE::CONSISTENCY, i)); } - const auto set_buffer = [this, &data_set](const auto & score_type) { - auto & old_score = buffer_.at(static_cast(score_type)); - auto new_score = data_set->get(score_type); - old_score.insert(old_score.end(), new_score.begin(), new_score.end()); - }; - - const auto clear_buffer = [this](const auto & score_type) { - buffer_.at(static_cast(score_type)).clear(); - }; - - if (count_ > 50) { - // plot(data_set); - clear_buffer(SCORE::LATERAL_COMFORTABILITY); - clear_buffer(SCORE::LONGITUDINAL_COMFORTABILITY); - clear_buffer(SCORE::EFFICIENCY); - clear_buffer(SCORE::SAFETY); - clear_buffer(SCORE::ACHIEVABILITY); - clear_buffer(SCORE::CONSISTENCY); - clear_buffer(SCORE::TOTAL); - count_ = 0; - } else { - set_buffer(SCORE::LATERAL_COMFORTABILITY); - set_buffer(SCORE::LONGITUDINAL_COMFORTABILITY); - set_buffer(SCORE::EFFICIENCY); - set_buffer(SCORE::SAFETY); - set_buffer(SCORE::ACHIEVABILITY); - set_buffer(SCORE::CONSISTENCY); - set_buffer(SCORE::TOTAL); - count_++; - } + // const auto set_buffer = [this, &data_set](const auto & score_type) { + // auto & old_score = buffer_.at(static_cast(score_type)); + // auto new_score = data_set->get(score_type); + // old_score.insert(old_score.end(), new_score.begin(), new_score.end()); + // }; + + // const auto clear_buffer = [this](const auto & score_type) { + // buffer_.at(static_cast(score_type)).clear(); + // }; + + // if (count_ > 50) { + // // plot(data_set); + // clear_buffer(SCORE::LATERAL_COMFORTABILITY); + // clear_buffer(SCORE::LONGITUDINAL_COMFORTABILITY); + // clear_buffer(SCORE::EFFICIENCY); + // clear_buffer(SCORE::SAFETY); + // clear_buffer(SCORE::ACHIEVABILITY); + // clear_buffer(SCORE::CONSISTENCY); + // // clear_buffer(SCORE::TOTAL); + // count_ = 0; + // } else { + // set_buffer(SCORE::LATERAL_COMFORTABILITY); + // set_buffer(SCORE::LONGITUDINAL_COMFORTABILITY); + // set_buffer(SCORE::EFFICIENCY); + // set_buffer(SCORE::SAFETY); + // set_buffer(SCORE::ACHIEVABILITY); + // set_buffer(SCORE::CONSISTENCY); + // // set_buffer(SCORE::TOTAL); + // count_++; + // } { autoware::universe_utils::appendMarkerArray( @@ -588,42 +588,42 @@ void BehaviorAnalyzerNode::on_timer() analyze(bag_data_); } -void BehaviorAnalyzerNode::plot(const std::shared_ptr & data_set) const -{ - const auto subplot = - [this](const auto & score_type, const size_t n_row, const size_t n_col, const size_t num) { - matplotlibcpp::subplot(n_row, n_col, num); - matplotlibcpp::hist(buffer_.at(static_cast(score_type)), 10); - matplotlibcpp::xlim(0.0, 1.0); - matplotlibcpp::ylim(0.0, 500.0); - std::stringstream ss; - ss << magic_enum::enum_name(score_type); - matplotlibcpp::title(ss.str()); - }; - - const auto plot_best = - [this](const auto & data, const size_t n_row, const size_t n_col, const size_t num) { - matplotlibcpp::subplot(n_row, n_col, num); - if (data != nullptr) { - matplotlibcpp::bar(data->score()); - } - matplotlibcpp::ylim(0.0, 1.0); - matplotlibcpp::title("BEST"); - }; - - const auto & p = parameters_; - - matplotlibcpp::clf(); - subplot(SCORE::LATERAL_COMFORTABILITY, 3, 3, 1); - subplot(SCORE::LONGITUDINAL_COMFORTABILITY, 3, 3, 2); - subplot(SCORE::EFFICIENCY, 3, 3, 3); - subplot(SCORE::SAFETY, 3, 3, 4); - subplot(SCORE::ACHIEVABILITY, 3, 3, 5); - subplot(SCORE::CONSISTENCY, 3, 3, 6); - subplot(SCORE::TOTAL, 3, 3, 7); - plot_best(data_set->best(p->weight), 3, 3, 8); - matplotlibcpp::pause(1e-9); -} +// void BehaviorAnalyzerNode::plot(const std::shared_ptr & data_set) const +// { +// const auto subplot = +// [this](const auto & score_type, const size_t n_row, const size_t n_col, const size_t num) { +// matplotlibcpp::subplot(n_row, n_col, num); +// matplotlibcpp::hist(buffer_.at(static_cast(score_type)), 10); +// matplotlibcpp::xlim(0.0, 1.0); +// matplotlibcpp::ylim(0.0, 500.0); +// std::stringstream ss; +// ss << magic_enum::enum_name(score_type); +// matplotlibcpp::title(ss.str()); +// }; + +// const auto plot_best = +// [this](const auto & data, const size_t n_row, const size_t n_col, const size_t num) { +// matplotlibcpp::subplot(n_row, n_col, num); +// if (data != nullptr) { +// matplotlibcpp::bar(data->score()); +// } +// matplotlibcpp::ylim(0.0, 1.0); +// matplotlibcpp::title("BEST"); +// }; + +// const auto & p = parameters_; + +// matplotlibcpp::clf(); +// subplot(SCORE::LATERAL_COMFORTABILITY, 3, 3, 1); +// subplot(SCORE::LONGITUDINAL_COMFORTABILITY, 3, 3, 2); +// subplot(SCORE::EFFICIENCY, 3, 3, 3); +// subplot(SCORE::SAFETY, 3, 3, 4); +// subplot(SCORE::ACHIEVABILITY, 3, 3, 5); +// subplot(SCORE::CONSISTENCY, 3, 3, 6); +// // subplot(SCORE::TOTAL, 3, 3, 7); +// // plot_best(data_set->best(), 3, 3, 8); +// matplotlibcpp::pause(1e-9); +// } } // namespace autoware::behavior_analyzer #include diff --git a/planning/autoware_planning_data_analyzer/src/node.hpp b/planning/autoware_planning_data_analyzer/src/node.hpp index 0ae724b0..8a229268 100644 --- a/planning/autoware_planning_data_analyzer/src/node.hpp +++ b/planning/autoware_planning_data_analyzer/src/node.hpp @@ -16,7 +16,6 @@ #define NODE_HPP_ #include "bag_handler.hpp" -#include "data_structs.hpp" #include "evaluation.hpp" #include "matplotlibcpp.h" #include "rosbag2_cpp/reader.hpp" @@ -87,7 +86,7 @@ class BehaviorAnalyzerNode : public rclcpp::Node rclcpp::Service::SharedPtr srv_route_; rclcpp::Service::SharedPtr srv_weight_; - vehicle_info_utils::VehicleInfo vehicle_info_; + std::shared_ptr vehicle_info_; std::shared_ptr bag_data_; diff --git a/planning/autoware_planning_data_analyzer/src/type_alias.hpp b/planning/autoware_planning_data_analyzer/src/type_alias.hpp index beff71e0..7d428e85 100644 --- a/planning/autoware_planning_data_analyzer/src/type_alias.hpp +++ b/planning/autoware_planning_data_analyzer/src/type_alias.hpp @@ -52,6 +52,7 @@ using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using autoware_vehicle_msgs::msg::SteeringReport; using route_handler::RouteHandler; +using vehicle_info_utils::VehicleInfo; using TrajectoryPoints = std::vector; diff --git a/planning/autoware_planning_data_analyzer/src/utils.cpp b/planning/autoware_planning_data_analyzer/src/utils.cpp index 5bea31d8..b6300748 100644 --- a/planning/autoware_planning_data_analyzer/src/utils.cpp +++ b/planning/autoware_planning_data_analyzer/src/utils.cpp @@ -109,8 +109,7 @@ double time_to_collision( auto convertToTrajectoryPoints( const autoware::sampler_common::Trajectory & trajectory, - const vehicle_info_utils::VehicleInfo & vehicle_info, const double z) - -> std::vector + const std::shared_ptr & vehicle_info, const double z) -> std::vector { std::vector traj_points; for (auto i = 0UL; i < trajectory.points.size(); ++i) { @@ -127,7 +126,7 @@ auto convertToTrajectoryPoints( p.longitudinal_velocity_mps = trajectory.longitudinal_velocities.at(i); p.lateral_velocity_mps = trajectory.lateral_velocities.at(i); p.acceleration_mps2 = trajectory.longitudinal_accelerations.at(i); - p.front_wheel_angle_rad = vehicle_info.wheel_base_m * trajectory.curvatures.at(i); + p.front_wheel_angle_rad = vehicle_info->wheel_base_m * trajectory.curvatures.at(i); traj_points.push_back(p); } return traj_points; @@ -221,8 +220,8 @@ auto resampling( auto sampling( const Trajectory & trajectory, const Pose & p_ego, const double v_ego, const double a_ego, - const vehicle_info_utils::VehicleInfo & vehicle_info, - const std::shared_ptr & parameters) -> std::vector> + const std::shared_ptr & vehicle_info, const std::shared_ptr & parameters) + -> std::vector> { const auto reference_trajectory = autoware::path_sampler::preparePathSpline(trajectory.points, true); @@ -285,8 +284,10 @@ auto sampling( auto to_marker( const std::shared_ptr & data, const SCORE & score_type, const size_t id) -> Marker { + if (data == nullptr) return {}; + const auto idx = static_cast(score_type); - const auto score = data->score().at(idx); + const auto score = data->scores().at(idx); std::stringstream ss; ss << magic_enum::enum_name(score_type); diff --git a/planning/autoware_planning_data_analyzer/src/utils.hpp b/planning/autoware_planning_data_analyzer/src/utils.hpp index 0eb150c4..150401e7 100644 --- a/planning/autoware_planning_data_analyzer/src/utils.hpp +++ b/planning/autoware_planning_data_analyzer/src/utils.hpp @@ -18,7 +18,6 @@ #include "autoware_frenet_planner/frenet_planner.hpp" #include "autoware_path_sampler/prepare_inputs.hpp" #include "autoware_path_sampler/utils/trajectory_utils.hpp" -#include "data_structs.hpp" #include "evaluation.hpp" #include "type_alias.hpp" @@ -43,7 +42,7 @@ double time_to_collision( auto convertToTrajectoryPoints( const autoware::sampler_common::Trajectory & trajectory, - const vehicle_info_utils::VehicleInfo & vehicle_info, const double z) + const std::shared_ptr & vehicle_info, const double z) -> std::vector; template @@ -62,8 +61,8 @@ auto resampling( auto sampling( const Trajectory & trajectory, const Pose & p_ego, const double v_ego, const double a_ego, - const vehicle_info_utils::VehicleInfo & vehicle_info, - const std::shared_ptr & parameters) -> std::vector>; + const std::shared_ptr & vehicle_info, const std::shared_ptr & parameters) + -> std::vector>; auto to_marker( const std::shared_ptr & data, const SCORE & score_type, const size_t id) -> Marker;