Skip to content

Commit

Permalink
refactor: clean up evaluator
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 28, 2024
1 parent fe433ba commit cff959b
Show file tree
Hide file tree
Showing 8 changed files with 251 additions and 276 deletions.
12 changes: 5 additions & 7 deletions planning/autoware_planning_data_analyzer/src/data_structs.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
};

Expand All @@ -41,7 +40,6 @@ enum class SCORE {
SAFETY = 3,
ACHIEVABILITY = 4,
CONSISTENCY = 5,
TOTAL = 6,
SIZE
};

Expand Down
256 changes: 111 additions & 145 deletions planning/autoware_planning_data_analyzer/src/evaluation.cpp
View file

Large diffs are not rendered by default.

83 changes: 47 additions & 36 deletions planning/autoware_planning_data_analyzer/src/evaluation.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,58 +32,66 @@ class DataInterface
public:
DataInterface(
const std::shared_ptr<BagData> & bag_data, const std::shared_ptr<TrajectoryPoints> & previous,
const vehicle_info_utils::VehicleInfo & vehicle_info,
const std::shared_ptr<VehicleInfo> & vehicle_info,
const std::shared_ptr<Parameters> & parameters, const std::string & tag,
const std::shared_ptr<TrajectoryPoints> & 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<double> & weight);

auto total(const std::vector<double> & weight) const -> double;
auto total() const -> double { return total_; };

double get(const SCORE & score_type) const;
bool feasible() const;

auto score() const -> std::vector<double> { 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<double> { return scores_; }

double minimum_ttc(const size_t idx) const;
auto points() const -> std::shared_ptr<TrajectoryPoints> { 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<TrajectoryPoints> { return points_; }
auto travel_distance(const size_t idx) const -> double;

std::shared_ptr<TrajectoryPoints> previous_;
auto lateral_deviation(const size_t idx) const -> double;

std::vector<PredictedObjects::SharedPtr> objects_history;
auto trajectory_deviation(const size_t idx) const -> double;

std::vector<std::vector<double>> values;
auto compress(const METRIC & metric_type) const -> double;

std::vector<double> scores;
std::vector<PredictedObjects::SharedPtr> objects_history;

vehicle_info_utils::VehicleInfo vehicle_info;
std::vector<std::vector<double>> values_;

std::shared_ptr<RouteHandler> route_handler;
std::vector<double> scores_;

std::shared_ptr<Parameters> parameters;
std::shared_ptr<TrajectoryPoints> previous_;

std::shared_ptr<TrajectoryPoints> points_;

std::string tag{""};
std::shared_ptr<VehicleInfo> vehicle_info_;

std::shared_ptr<RouteHandler> route_handler_;

std::shared_ptr<Parameters> parameters_;

std::string tag_;

double total_;
};

class GroundTruth : public DataInterface
Expand All @@ -92,7 +100,7 @@ class GroundTruth : public DataInterface
GroundTruth(
const std::shared_ptr<BagData> & bag_data,
const std::shared_ptr<TrajectoryPoints> & prev_best_data,
const vehicle_info_utils::VehicleInfo & vehicle_info,
const std::shared_ptr<VehicleInfo> & vehicle_info,
const std::shared_ptr<Parameters> & parameters, const std::string & tag);

private:
Expand All @@ -107,7 +115,7 @@ class TrajectoryData : public DataInterface
TrajectoryData(
const std::shared_ptr<BagData> & bag_data,
const std::shared_ptr<TrajectoryPoints> & prev_best_data,
const vehicle_info_utils::VehicleInfo & vehicle_info,
const std::shared_ptr<VehicleInfo> & vehicle_info,
const std::shared_ptr<Parameters> & parameters, const std::string & tag,
const std::shared_ptr<TrajectoryPoints> & points);
};
Expand All @@ -118,26 +126,29 @@ class Evaluator
Evaluator(
const std::shared_ptr<BagData> & bag_data,
const std::shared_ptr<TrajectoryPoints> & prev_best_data,
const vehicle_info_utils::VehicleInfo & vehicle_info,
const std::shared_ptr<VehicleInfo> & vehicle_info,
const std::shared_ptr<Parameters> & parameters);

void show();

void normalize_scores();
void show() const;

auto loss(const std::vector<double> & weight) const -> double;
auto results() const -> std::vector<std::shared_ptr<DataInterface>> { return results_; }

auto get(const SCORE & score_type) const -> std::vector<double>;
auto best(const std::vector<double> & weight) -> std::shared_ptr<DataInterface>;

auto best(const std::vector<double> & weight) const -> std::shared_ptr<DataInterface>;
auto loss(const std::vector<double> & weight) -> double;

auto get(const std::string & tag) const -> std::shared_ptr<DataInterface>;

std::vector<std::shared_ptr<DataInterface>> data_set;
private:
void normalize();

void weighting(const std::vector<double> & weight);

auto best() const -> std::shared_ptr<DataInterface>;

std::shared_ptr<RouteHandler> route_handler;
std::vector<std::shared_ptr<DataInterface>> results_;

std::shared_ptr<Parameters> parameters;
std::shared_ptr<Parameters> parameters_;
};

} // namespace autoware::behavior_analyzer
Expand Down
152 changes: 76 additions & 76 deletions planning/autoware_planning_data_analyzer/src/node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<VehicleInfo>(
autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo());

pub_marker_ = create_publisher<MarkerArray>("~/marker", 1);
pub_odometry_ = create_publisher<Odometry>(TOPIC::ODOMETRY, rclcpp::QoS(1));
Expand Down Expand Up @@ -526,47 +527,46 @@ void BehaviorAnalyzerNode::visualize(const std::shared_ptr<Evaluator> & 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<size_t>(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<size_t>(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<size_t>(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<size_t>(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(
Expand All @@ -588,42 +588,42 @@ void BehaviorAnalyzerNode::on_timer()
analyze(bag_data_);
}

void BehaviorAnalyzerNode::plot(const std::shared_ptr<Evaluator> & 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<size_t>(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<double>(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<Evaluator> & 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<size_t>(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<double>(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 <rclcpp_components/register_node_macro.hpp>
Expand Down
3 changes: 1 addition & 2 deletions planning/autoware_planning_data_analyzer/src/node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -87,7 +86,7 @@ class BehaviorAnalyzerNode : public rclcpp::Node
rclcpp::Service<Trigger>::SharedPtr srv_route_;
rclcpp::Service<Trigger>::SharedPtr srv_weight_;

vehicle_info_utils::VehicleInfo vehicle_info_;
std::shared_ptr<VehicleInfo> vehicle_info_;

std::shared_ptr<BagData> bag_data_;

Expand Down
1 change: 1 addition & 0 deletions planning/autoware_planning_data_analyzer/src/type_alias.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<TrajectoryPoint>;

Expand Down
Loading

0 comments on commit cff959b

Please sign in to comment.