diff --git a/planning/autoware_planning_data_analyzer/launch/behavior_analyzer.launch.xml b/planning/autoware_planning_data_analyzer/launch/behavior_analyzer.launch.xml index 4e9e0149..0cb1e2a4 100644 --- a/planning/autoware_planning_data_analyzer/launch/behavior_analyzer.launch.xml +++ b/planning/autoware_planning_data_analyzer/launch/behavior_analyzer.launch.xml @@ -29,7 +29,7 @@ - + diff --git a/planning/autoware_planning_data_analyzer/src/data_structs.cpp b/planning/autoware_planning_data_analyzer/src/data_structs.cpp index 5b4d6c43..4c6295ba 100644 --- a/planning/autoware_planning_data_analyzer/src/data_structs.cpp +++ b/planning/autoware_planning_data_analyzer/src/data_structs.cpp @@ -157,6 +157,7 @@ void CommonData::calculate() 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; @@ -164,6 +165,7 @@ void CommonData::calculate() 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)); @@ -171,6 +173,7 @@ void CommonData::calculate() 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)); } { @@ -179,6 +182,7 @@ void CommonData::calculate() 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)); } values.at(static_cast(METRIC::LATERAL_ACCEL)) = lateral_accel_values; @@ -186,6 +190,7 @@ void CommonData::calculate() 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)) = lateral_comfortability(); scores.at(static_cast(SCORE::LONGITUDINAL_COMFORTABILITY)) = @@ -193,6 +198,7 @@ void CommonData::calculate() scores.at(static_cast(SCORE::EFFICIENCY)) = efficiency(); scores.at(static_cast(SCORE::SAFETY)) = safety(); scores.at(static_cast(SCORE::ACHIEVABILITY)) = achievability(); + scores.at(static_cast(SCORE::CONSISTENCY)) = consistency(); } void CommonData::normalize( @@ -271,6 +277,20 @@ double CommonData::achievability() const return score; } +double CommonData::consistency() const +{ + constexpr double TIME_FACTOR = 1.0; + + 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::TRAJECTORY_DEVIATION)).at(i)); + } + + return score; +} + double CommonData::get(const SCORE & score_type) const { return scores.at(static_cast(score_type)); @@ -283,7 +303,8 @@ double CommonData::total( w1 * scores.at(static_cast(SCORE::LONGITUDINAL_COMFORTABILITY)) + w2 * scores.at(static_cast(SCORE::EFFICIENCY)) + w3 * scores.at(static_cast(SCORE::SAFETY)) + - w4 * scores.at(static_cast(SCORE::ACHIEVABILITY)); + w4 * scores.at(static_cast(SCORE::ACHIEVABILITY)) + + 1.0 * scores.at(static_cast(SCORE::CONSISTENCY)); } ManualDrivingData::ManualDrivingData( @@ -378,6 +399,11 @@ double ManualDrivingData::lateral_deviation(const size_t idx) const return arc_coordinates.distance; } +double ManualDrivingData::trajectory_deviation([[maybe_unused]] const size_t idx) const +{ + return 1.0; +} + bool ManualDrivingData::ready() const { if (objects_history.size() < parameters->resample_num) { @@ -402,8 +428,8 @@ bool ManualDrivingData::ready() const TrajectoryData::TrajectoryData( const std::shared_ptr & bag_data, const vehicle_info_utils::VehicleInfo & vehicle_info, const std::shared_ptr & parameters, const std::string & tag, - const std::vector & points) -: CommonData(bag_data, vehicle_info, parameters, tag), points{points} + const std::vector & points, const std::optional & t_best) +: CommonData(bag_data, vehicle_info, parameters, tag), points{points}, t_best{t_best} { calculate(); } @@ -445,6 +471,15 @@ double TrajectoryData::lateral_deviation(const size_t idx) const return arc_coordinates.distance; } +double TrajectoryData::trajectory_deviation(const size_t idx) const +{ + if (!t_best.has_value()) return 0.0; + + const auto & p1 = autoware::universe_utils::getPose(points.at(idx)); + const auto & p2 = t_best.value().at(idx); + return autoware::universe_utils::calcSquaredDistance2d(p1, p2); +} + bool TrajectoryData::feasible() const { const auto condition = [](const auto & p) { return p.longitudinal_velocity_mps >= 0.0; }; @@ -466,7 +501,7 @@ bool TrajectoryData::ready() const SamplingTrajectoryData::SamplingTrajectoryData( const std::shared_ptr & bag_data, const vehicle_info_utils::VehicleInfo & vehicle_info, - const std::shared_ptr & parameters) + const std::shared_ptr & parameters, const std::optional & t_best) { const auto opt_odometry = std::dynamic_pointer_cast>( bag_data->buffers.at("/localization/kinematic_state")) @@ -497,7 +532,7 @@ SamplingTrajectoryData::SamplingTrajectoryData( for (const auto & sample : utils::sampling( *opt_trajectory, opt_odometry->pose.pose, opt_odometry->twist.twist.linear.x, opt_accel->accel.accel.linear.x, vehicle_info, parameters)) { - data.emplace_back(bag_data, vehicle_info, parameters, "frenet", sample); + data.emplace_back(bag_data, vehicle_info, parameters, "frenet", sample, t_best); } // std::vector stop_points(parameters->resample_num); diff --git a/planning/autoware_planning_data_analyzer/src/data_structs.hpp b/planning/autoware_planning_data_analyzer/src/data_structs.hpp index b55f5ed9..7821973c 100644 --- a/planning/autoware_planning_data_analyzer/src/data_structs.hpp +++ b/planning/autoware_planning_data_analyzer/src/data_structs.hpp @@ -38,6 +38,7 @@ enum class METRIC { TRAVEL_DISTANCE = 3, MINIMUM_TTC = 4, LATERAL_DEVIATION = 5, + TRAJECTORY_DEVIATION = 6, SIZE }; @@ -47,7 +48,8 @@ enum class SCORE { EFFICIENCY = 2, SAFETY = 3, ACHIEVABILITY = 4, - TOTAL = 5, + CONSISTENCY = 5, + TOTAL = 6, SIZE }; @@ -242,6 +244,8 @@ struct CommonData double achievability() const; + double consistency() const; + double total( const double w0, const double w1, const double w2, const double w3, const double w4) const; @@ -259,6 +263,8 @@ struct CommonData virtual double lateral_deviation(const size_t idx) const = 0; + virtual double trajectory_deviation(const size_t idx) const = 0; + virtual bool feasible() const = 0; virtual bool ready() const = 0; @@ -294,6 +300,8 @@ struct ManualDrivingData : CommonData double lateral_deviation(const size_t idx) const override; + double trajectory_deviation(const size_t idx) const override; + bool feasible() const override { return true; } bool ready() const override; @@ -308,7 +316,7 @@ struct TrajectoryData : CommonData TrajectoryData( const std::shared_ptr & bag_data, const vehicle_info_utils::VehicleInfo & vehicle_info, const std::shared_ptr & parameters, const std::string & tag, - const std::vector & points); + const std::vector & points, const std::optional & t_best); double lateral_accel(const size_t idx) const override; @@ -320,18 +328,22 @@ struct TrajectoryData : CommonData double lateral_deviation(const size_t idx) const override; + double trajectory_deviation(const size_t idx) const override; + bool feasible() const override; bool ready() const override; std::vector points; + + std::optional t_best; }; struct SamplingTrajectoryData { SamplingTrajectoryData( const std::shared_ptr & bag_data, const vehicle_info_utils::VehicleInfo & vehicle_info, - const std::shared_ptr & parameters); + const std::shared_ptr & parameters, const std::optional & t_best); auto best(const double w0, const double w1, const double w2, const double w3, const double w4) const -> std::optional @@ -370,9 +382,9 @@ struct DataSet { DataSet( const std::shared_ptr & bag_data, const vehicle_info_utils::VehicleInfo & vehicle_info, - const std::shared_ptr & parameters) + const std::shared_ptr & parameters, const std::optional & t_best) : manual{ManualDrivingData(bag_data, vehicle_info, parameters)}, - sampling{SamplingTrajectoryData(bag_data, vehicle_info, parameters)}, + sampling{SamplingTrajectoryData(bag_data, vehicle_info, parameters, t_best)}, route_handler{bag_data->route_handler}, parameters{parameters} { @@ -397,6 +409,7 @@ struct DataSet const auto [s2_min, s2_max] = range(static_cast(SCORE::EFFICIENCY)); const auto [s3_min, s3_max] = range(static_cast(SCORE::SAFETY)); 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 : sampling.data) { data.normalize(s0_min, s0_max, static_cast(SCORE::LATERAL_COMFORTABILITY), true); @@ -404,6 +417,7 @@ struct DataSet 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->w0, parameters->w1, parameters->w2, parameters->w3, parameters->w4); } @@ -458,6 +472,8 @@ struct DataSet double s3_dev = 0.0; double s4_ave = 0.0; double s4_dev = 0.0; + double s5_ave = 0.0; + double s5_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); @@ -474,6 +490,7 @@ struct DataSet 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); } std::stringstream ss; @@ -485,6 +502,7 @@ struct DataSet ss << " efficiency :" << best.value().get(SCORE::EFFICIENCY) << " mean:" << s2_ave << " std:" << std::sqrt(s2_dev) << "\n"; // NOLINT ss << " safety :" << best.value().get(SCORE::SAFETY) << " mean:" << s3_ave << " std:" << std::sqrt(s3_dev) << "\n"; // NOLINT ss << " achievability :" << best.value().get(SCORE::ACHIEVABILITY) << " mean:" << s4_ave << " std:" << std::sqrt(s4_dev) << "\n"; // NOLINT + ss << " consistency :" << best.value().get(SCORE::CONSISTENCY) << " mean:" << s5_ave << " std:" << std::sqrt(s5_dev) << "\n"; // NOLINT ss << " total :" << best.value().get(SCORE::TOTAL); // clang-format on RCLCPP_INFO_STREAM(rclcpp::get_logger(__func__), ss.str()); diff --git a/planning/autoware_planning_data_analyzer/src/node.cpp b/planning/autoware_planning_data_analyzer/src/node.cpp index ea9dee5c..ace6cdbb 100644 --- a/planning/autoware_planning_data_analyzer/src/node.cpp +++ b/planning/autoware_planning_data_analyzer/src/node.cpp @@ -31,6 +31,7 @@ using autoware::universe_utils::Polygon2d; BehaviorAnalyzerNode::BehaviorAnalyzerNode(const rclcpp::NodeOptions & node_options) : Node("path_selector_node", node_options), route_handler_{std::make_shared()}, + best_{std::nullopt}, buffer_{static_cast(SCORE::SIZE)} { using namespace std::literals::chrono_literals; @@ -317,12 +318,14 @@ void BehaviorAnalyzerNode::weight( RCLCPP_INFO_STREAM(get_logger(), ss.str()); }; + std::optional best{std::nullopt}; + while (reader_.has_next() && rclcpp::ok()) { update(bag_data, p->grid_seach.dt); if (!bag_data->ready()) break; - const auto data_set = std::make_shared(bag_data, vehicle_info_, p); + const auto data_set = std::make_shared(bag_data, vehicle_info_, p, best); std::mutex grid_mutex; @@ -382,7 +385,7 @@ void BehaviorAnalyzerNode::analyze(const std::shared_ptr & bag_data) co { if (!bag_data->ready()) return; - const auto data_set = std::make_shared(bag_data, vehicle_info_, parameters_); + const auto data_set = std::make_shared(bag_data, vehicle_info_, parameters_, best_); const auto opt_tf = std::dynamic_pointer_cast>(bag_data->buffers.at(TOPIC::TF)) ->get(bag_data->timestamp); @@ -523,6 +526,9 @@ void BehaviorAnalyzerNode::visualize(const std::shared_ptr & data_set) marker.points.push_back(point.pose.position); } msg.markers.push_back(marker); + best_ = best.value().points; + } else { + best_ = std::nullopt; } for (size_t i = 0; i < data_set->sampling.data.size(); ++i) { @@ -532,6 +538,7 @@ void BehaviorAnalyzerNode::visualize(const std::shared_ptr & data_set) 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)); } @@ -552,6 +559,7 @@ void BehaviorAnalyzerNode::visualize(const std::shared_ptr & data_set) clear_buffer(SCORE::EFFICIENCY); clear_buffer(SCORE::SAFETY); clear_buffer(SCORE::ACHIEVABILITY); + clear_buffer(SCORE::CONSISTENCY); clear_buffer(SCORE::TOTAL); count_ = 0; } else { @@ -560,6 +568,7 @@ void BehaviorAnalyzerNode::visualize(const std::shared_ptr & data_set) set_buffer(SCORE::EFFICIENCY); set_buffer(SCORE::SAFETY); set_buffer(SCORE::ACHIEVABILITY); + set_buffer(SCORE::CONSISTENCY); set_buffer(SCORE::TOTAL); count_++; } @@ -615,8 +624,9 @@ void BehaviorAnalyzerNode::plot(const std::shared_ptr & data_set) const subplot(SCORE::EFFICIENCY, 3, 3, 3); subplot(SCORE::SAFETY, 3, 3, 4); subplot(SCORE::ACHIEVABILITY, 3, 3, 5); - subplot(SCORE::TOTAL, 3, 3, 6); - plot_best(data_set->sampling.best(p->w0, p->w1, p->w2, p->w3, p->w4), 3, 3, 7); + subplot(SCORE::CONSISTENCY, 3, 3, 6); + subplot(SCORE::TOTAL, 3, 3, 7); + plot_best(data_set->sampling.best(p->w0, p->w1, p->w2, p->w3, p->w4), 3, 3, 8); matplotlibcpp::pause(1e-9); } } // namespace autoware::behavior_analyzer diff --git a/planning/autoware_planning_data_analyzer/src/node.hpp b/planning/autoware_planning_data_analyzer/src/node.hpp index 5d22ea7a..cfdc7178 100644 --- a/planning/autoware_planning_data_analyzer/src/node.hpp +++ b/planning/autoware_planning_data_analyzer/src/node.hpp @@ -93,6 +93,8 @@ class BehaviorAnalyzerNode : public rclcpp::Node std::shared_ptr parameters_; + mutable std::optional best_; + mutable std::mutex mutex_; mutable std::vector> buffer_; diff --git a/planning/autoware_planning_data_analyzer/src/type_alias.hpp b/planning/autoware_planning_data_analyzer/src/type_alias.hpp index 53b0e443..4cc9507c 100644 --- a/planning/autoware_planning_data_analyzer/src/type_alias.hpp +++ b/planning/autoware_planning_data_analyzer/src/type_alias.hpp @@ -50,6 +50,8 @@ using autoware_planning_msgs::msg::TrajectoryPoint; using autoware_vehicle_msgs::msg::SteeringReport; using route_handler::RouteHandler; +using TrajectoryPoints = std::vector; + // ros2 using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Point;